From 91b688de212036d2c33b15437652dff156a4ff66 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 20 Sep 2023 13:29:32 -0700 Subject: [PATCH] [MPPI] complete minor optimaization with floating point calculations (#3827) * floating point calculations * Update optimizer_unit_tests.cpp * Update critics_tests.cpp * Update critics_tests.cpp --- nav2_mppi_controller/CMakeLists.txt | 3 +- .../critics/obstacles_critic.hpp | 4 +- .../critics/path_angle_critic.hpp | 2 +- .../models/constraints.hpp | 14 +++--- .../tools/path_handler.hpp | 4 +- .../nav2_mppi_controller/tools/utils.hpp | 44 +++++++++---------- .../src/critics/obstacles_critic.cpp | 14 +++--- .../src/critics/path_align_critic.cpp | 2 +- nav2_mppi_controller/src/noise_generator.cpp | 6 +-- nav2_mppi_controller/src/path_handler.cpp | 8 ++-- nav2_mppi_controller/test/critics_tests.cpp | 8 ++-- .../test/optimizer_unit_tests.cpp | 12 ++--- 12 files changed, 62 insertions(+), 59 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 9c3e5f6fa30..5cc8a8c0e90 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -47,7 +47,8 @@ if(COMPILER_SUPPORTS_FMA) add_compile_options(-mfma) endif() -add_compile_options(-O3 -finline-limit=1000000 -ffp-contract=fast -ffast-math) +# If building one the same hardware to be deployed on, try `-march=native`! +add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) add_library(mppi_controller SHARED src/controller.cpp diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index 37e14cb02c6..fe17906bae4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -79,14 +79,14 @@ class ObstaclesCritic : public CriticFunction * @return double circumscribed cost, any higher than this and need to do full footprint collision checking * since some element of the robot could be in collision */ - double findCircumscribedCost(std::shared_ptr costmap); + float findCircumscribedCost(std::shared_ptr costmap); protected: nav2_costmap_2d::FootprintCollisionChecker collision_checker_{nullptr}; bool consider_footprint_{true}; - double collision_cost_{0}; + float collision_cost_{0}; float inflation_scale_factor_{0}, inflation_radius_{0}; float possibly_inscribed_cost_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index f7ba486a0c4..b137270aba9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -72,7 +72,7 @@ class PathAngleCritic : public CriticFunction void score(CriticData & data) override; protected: - double max_angle_to_furthest_{0}; + float max_angle_to_furthest_{0}; float threshold_to_consider_{0}; size_t offset_from_furthest_{0}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp index b7f9b6f3ccb..5e1885c271f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp @@ -24,10 +24,10 @@ namespace mppi::models */ struct ControlConstraints { - double vx_max; - double vx_min; - double vy; - double wz; + float vx_max; + float vx_min; + float vy; + float wz; }; /** @@ -36,9 +36,9 @@ struct ControlConstraints */ struct SamplingStd { - double vx; - double vy; - double wz; + float vx; + float vy; + float wz; }; } // namespace mppi::models diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index 468e4964a40..eeb28fa01b3 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -151,8 +151,8 @@ class PathHandler double max_robot_pose_search_dist_{0}; double prune_distance_{0}; double transform_tolerance_{0}; - double inversion_xy_tolerance_{0.2}; - double inversion_yaw_tolerance{0.4}; + float inversion_xy_tolerance_{0.2}; + float inversion_yaw_tolerance{0.4}; bool enforce_path_inversion_{false}; unsigned int inversion_locale_{0u}; }; 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 ce2d0570c0c..6baa27873eb 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -309,7 +309,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) const auto dists = dx * dx + dy * dy; size_t max_id_by_trajectories = 0; - double min_distance_by_path = std::numeric_limits::max(); + float min_distance_by_path = std::numeric_limits::max(); for (size_t i = 0; i < dists.shape(0); i++) { size_t min_id_by_path = 0; @@ -337,7 +337,7 @@ inline size_t findPathTrajectoryInitialPoint(const CriticData & data) const auto dy = data.path.y - data.trajectories.y(0, 0); const auto dists = dx * dx + dy * dy; - double min_distance_by_path = std::numeric_limits::max(); + float min_distance_by_path = std::numeric_limits::max(); size_t min_id = 0; for (size_t j = 0; j < dists.shape(0); j++) { if (dists(j) < min_distance_by_path) { @@ -420,23 +420,23 @@ inline void setPathCostsIfNotSet( * @param forward_preference If reversing direction is valid * @return Angle between two points */ -inline double posePointAngle( +inline float posePointAngle( const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) { - double pose_x = pose.position.x; - double pose_y = pose.position.y; - double pose_yaw = tf2::getYaw(pose.orientation); + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); - double yaw = atan2(point_y - pose_y, point_x - pose_x); + float yaw = atan2f(point_y - pose_y, point_x - pose_x); // If no preference for forward, return smallest angle either in heading or 180 of heading if (!forward_preference) { return std::min( - abs(angles::shortest_angular_distance(yaw, pose_yaw)), - abs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); + fabs(angles::shortest_angular_distance(yaw, pose_yaw)), + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); } - return abs(angles::shortest_angular_distance(yaw, pose_yaw)); + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); } /** @@ -447,21 +447,21 @@ inline double posePointAngle( * @param point_yaw Yaw of the point to consider along Z axis * @return Angle between two points */ -inline double posePointAngle( +inline float posePointAngle( const geometry_msgs::msg::Pose & pose, double point_x, double point_y, double point_yaw) { - double pose_x = pose.position.x; - double pose_y = pose.position.y; - double pose_yaw = tf2::getYaw(pose.orientation); + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); - double yaw = atan2(point_y - pose_y, point_x - pose_x); + float yaw = atan2f(point_y - pose_y, point_x - pose_x); - if (abs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) { + if (fabs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) { yaw = angles::normalize_angle(yaw + M_PI); } - return abs(angles::shortest_angular_distance(yaw, pose_yaw)); + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); } /** @@ -650,17 +650,17 @@ inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) // Iterating through the path to determine the position of the path inversion for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = path.poses[idx].pose.position.x - + float oa_x = path.poses[idx].pose.position.x - path.poses[idx - 1].pose.position.x; - double oa_y = path.poses[idx].pose.position.y - + float oa_y = path.poses[idx].pose.position.y - path.poses[idx - 1].pose.position.y; - double ab_x = path.poses[idx + 1].pose.position.x - + float ab_x = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x; - double ab_y = path.poses[idx + 1].pose.position.y - + float ab_y = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y; // Checking for the existance of cusp, in the path, using the dot product. - double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); if (dot_product < 0.0) { return idx + 1; } diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index b9225635971..80ac77e10f5 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -32,7 +32,7 @@ void ObstaclesCritic::initialize() collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); - if (possibly_inscribed_cost_ < 1) { + if (possibly_inscribed_cost_ < 1.0f) { RCLCPP_ERROR( logger_, "Inflation layer either not found or inflation is not set sufficiently for " @@ -50,7 +50,7 @@ void ObstaclesCritic::initialize() "footprint" : "circular"); } -double ObstaclesCritic::findCircumscribedCost( +float ObstaclesCritic::findCircumscribedCost( std::shared_ptr costmap) { double result = -1.0; @@ -75,7 +75,7 @@ double ObstaclesCritic::findCircumscribedCost( if (!inflation_layer_found) { RCLCPP_WARN( - rclcpp::get_logger("computeCircumscribedCost"), + logger_, "No inflation layer found in costmap configuration. " "If this is an SE2-collision checking plugin, it cannot use costmap potential " "field to speed up collision checking by only checking the full footprint " @@ -83,7 +83,7 @@ double ObstaclesCritic::findCircumscribedCost( "significantly slow down planning times and not avoid anything but absolute collisions!"); } - return result; + return static_cast(result); } float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) @@ -137,7 +137,7 @@ void ObstaclesCritic::score(CriticData & data) } // Cannot process repulsion if inflation layer does not exist - if (inflation_radius_ == 0 || inflation_scale_factor_ == 0) { + if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) { continue; } @@ -197,7 +197,9 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) } cost = collision_checker_.pointCost(x_i, y_i); - if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) { + if (consider_footprint_ && + (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) + { cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); collision_cost.using_footprint = true; diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 795f166c1ad..4ff12d4e553 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -89,7 +89,7 @@ void PathAlignCritic::score(CriticData & data) } float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; - double min_dist_sq = std::numeric_limits::max(); + float min_dist_sq = std::numeric_limits::max(); size_t min_s = 0; for (size_t t = 0; t < batch_size; ++t) { diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 60173789f0d..09ee4ab92d7 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -94,14 +94,14 @@ void NoiseGenerator::generateNoisedControls() auto & s = settings_; xt::noalias(noises_vx_) = xt::random::randn( - {s.batch_size, s.time_steps}, 0.0, + {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.0, + {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.0, + {s.batch_size, s.time_steps}, 0.0f, s.sampling_std.vy); } } diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index f1022513f2b..ddf2b403b68 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -166,8 +166,8 @@ bool PathHandler::transformPose( double PathHandler::getMaxCostmapDist() { const auto & costmap = costmap_->getCostmap(); - return std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * - costmap->getResolution() / 2.0; + return static_cast(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) * + costmap->getResolution() * 0.50; } void PathHandler::setPath(const nav_msgs::msg::Path & plan) @@ -190,11 +190,11 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam { // Keep full path if we are within tolerance of the inversion pose const auto last_pose = global_plan_up_to_inversion_.poses.back(); - double distance = std::hypot( + float distance = hypotf( robot_pose.pose.position.x - last_pose.pose.position.x, robot_pose.pose.position.y - last_pose.pose.position.y); - double angle_distance = angles::shortest_angular_distance( + float angle_distance = angles::shortest_angular_distance( tf2::getYaw(robot_pose.pose.orientation), tf2::getYaw(last_pose.pose.orientation)); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index ce2896122c1..8b5efae8da4 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -380,19 +380,19 @@ TEST(CriticTests, PreferForwardCritic) path.reset(10); path.x(9) = 10.0; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(xt::sum(costs, immediate)(), 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}); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(xt::sum(costs, immediate)(), 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}); critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0.0); - EXPECT_NEAR(costs(0), 15.0, 1e-6); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length + EXPECT_GT(xt::sum(costs, immediate)(), 0.0f); + EXPECT_NEAR(costs(0), 15.0f, 1e-3f); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length } TEST(CriticTests, TwirlingCritic) diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index f27471d28a3..7c7f8a76914 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -432,20 +432,20 @@ TEST(OptimizerTests, SpeedLimitTests) // Test Speed limits API auto [v_min, v_max] = optimizer_tester.getVelLimits(); - EXPECT_EQ(v_max, 0.5); - EXPECT_EQ(v_min, -0.35); + EXPECT_EQ(v_max, 0.5f); + EXPECT_EQ(v_min, -0.35f); optimizer_tester.setSpeedLimit(0, false); auto [v_min2, v_max2] = optimizer_tester.getVelLimits(); - EXPECT_EQ(v_max2, 0.5); - EXPECT_EQ(v_min2, -0.35); + EXPECT_EQ(v_max2, 0.5f); + EXPECT_EQ(v_min2, -0.35f); optimizer_tester.setSpeedLimit(50.0, true); auto [v_min3, v_max3] = optimizer_tester.getVelLimits(); EXPECT_NEAR(v_max3, 0.5 / 2.0, 1e-3); EXPECT_NEAR(v_min3, -0.35 / 2.0, 1e-3); optimizer_tester.setSpeedLimit(0, true); auto [v_min4, v_max4] = optimizer_tester.getVelLimits(); - EXPECT_EQ(v_max4, 0.5); - EXPECT_EQ(v_min4, -0.35); + EXPECT_EQ(v_max4, 0.5f); + EXPECT_EQ(v_min4, -0.35f); optimizer_tester.setSpeedLimit(0.75, false); auto [v_min5, v_max5] = optimizer_tester.getVelLimits(); EXPECT_NEAR(v_max5, 0.75, 1e-3);