Can MPPI follow the path with time stamps?

[Chortine]

Hi team,

I have a global path planner that outputs the points with time stamps, and the car is supposed to be a the pose at the suggested exact time. I would like to know that if MPPI is able to do such local planning with strict time critics.

I look into the PathFollowCritcs and PathAlignCritics, however, there seems to be no items encourging 'being at the pose at time'. Maybe I've missed something. Can we extend MPPI to do that? Tons of thanks.

[SteveMacenski]

At this moment, no, but MPPI is pluginized for a reason, you could easily create a new PathFollower/PathAlign critic that takes into account the timing in your pose messages. The MPPI samples themselves have a time forward, velocity, and pose information, so if you path contains the timing information, you'd have all of the information required in front of you - you just need to develop the algorithm to do so.

I think this would also be a great option to provide to MPPI users in the future as well, so if you created/adapted a path following critic for using path timestamps, I'd be more than happy to merge, review, and maintain it in Nav2 itself!

[Chortine]

At this moment, no, but MPPI is pluginized for a reason, you could easily create a new PathFollower/PathAlign critic that takes into account the timing in your pose messages. The MPPI samples themselves have a time forward, velocity, and pose information, so if you path contains the timing information, you'd have all of the information required in front of you - you just need to develop the algorithm to do so.

I think this would also be a great option to provide to MPPI users in the future as well, so if you created/adapted a path following critic for using path timestamps, I'd be more than happy to merge, review, and maintain it in Nav2 itself!

Sure! I'll work on it these days, and when it passes our case, I will submit a merge request ;)

[SteveMacenski]

@Chortine any updates or problems? 😄

[Chortine]

Indeed, I'm facing some problems. I have implemented my first version of time_critic, the car now drive faster than without this critic. However, it starts zig-zag around the path, instead of nicely aligning with the path like before, specially when the curve of the path is large. I'm currently working to solve this. Do you have any suggestions?

Screencast.2024-03-28.14.17.19.mp4

[SteveMacenski]

Its hard to say without looking at the code and the video is corrupted so its not viewable on my side. I'd suspect you need to run the controller faster to better converge

[Chortine]

Hi,

Here is my implementation of time_critic.cpp:

    using namespace xt::placeholders; // NOLINT
    using xt::evaluation_strategy::immediate;

    void TimeCritic::initialize()
    {
        auto getParam = parameters_handler_->getParamGetter(name_);
        getParam(path_dt_, "path_dt", 0.1f);
        getParam(power_, "cost_power", 1);
        getParam(weight_, "cost_weight", 10.0);
        getParam(weight_1_, "cost_weight_1", 10.0);
        getParam(weight_2_, "cost_weight_2", 10.0);

        RCLCPP_INFO(
            logger_,
            "TimeCritic instantiated with %d power and %f weight",
            power_, weight_);
    }

    void TimeCritic::score(CriticData &data)
    {
        std::cout << "============ goes into TimeCritic ============" << std::endl;
        // print the time of pose stamped in data
        std::cout << "============ pose time: " << data.state.pose.header.stamp.sec << "." << data.state.pose.header.stamp.nanosec << std::endl;
        float control_dt = data.model_dt;

        utils::setPathFurthestPointIfNotSet(data);
        const size_t path_segments_count = *data.furthest_reached_path_point; // up to furthest only

        if (path_segments_count < 2) // offset_from_furthest_
        {
            return;
        }
        // std::cout << "============ [TimeCritic] ckpt1" << std::endl;

        const auto P_x = xt::view(data.path.x, xt::range(_, -1));      // path points
        const auto P_y = xt::view(data.path.y, xt::range(_, -1));      // path points
        const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points

        std::cout << "P_x shape: " << P_x.shape(0) << ", " << P_x.shape(1) << std::endl;

        std::cout << "============ P_x, P_y: " << std::endl;
        for (unsigned int i = 1; i < P_x.shape(0); i++)
        {
            std::cout << P_x(i) << ", " << P_y(i) << ";  ";
        }
        std::cout << "============ " << std::endl;

        const size_t batch_size = data.trajectories.x.shape(0);
        const size_t time_steps = data.trajectories.x.shape(1);
        auto &&cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
        // std::cout << "============ [TimeCritic] ckpt2" << std::endl;
        // Find integrated distance in the path
        std::vector<float> path_integrated_distances(path_segments_count, 0.0f);
        float dx = 0.0f, dy = 0.0f, dyaw = 0.0f;

        for (unsigned int i = 1; i != path_segments_count; i++)
        {
            dx = P_x(i) - P_x(i - 1);
            dy = P_y(i) - P_y(i - 1);
            float curr_dist = sqrtf(dx * dx + dy * dy);
            path_integrated_distances[i] = path_integrated_distances[i - 1] + curr_dist;
        }
        // std::cout << "============ [TimeCritic] ckpt3" << std::endl;
        size_t step_interval = path_dt_ / control_dt; // 0.1 / 0.05
        float Tx = 0.0f, Ty = 0.0f;
        size_t path_pt = 0;
        float num_samples = 0.0f;
        float summed_path_cost = 0.0f;
        // float lambda = 0.0f;
        for (size_t t = 0; t < batch_size; ++t)
        {
            // std::cout << "============ [TimeCritic] ckpt4" << std::endl;
            // path_pt = utils::findClosestPathPt(
            //     path_integrated_distances, 0.0, 0);
            path_pt = 0;
            num_samples = 0.0f;
            summed_path_cost = 0.0f;
            const auto T_x = xt::view(data.trajectories.x, t, xt::all());
            const auto T_y = xt::view(data.trajectories.y, t, xt::all());
            const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all());

            // std::cout << "============ [TimeCritic] ckpt4.5" << std::endl;
            for (size_t p = step_interval; p < time_steps; p += step_interval)
            {
                // lambda = 1.0f - static_cast<float>(p) / time_steps;

                path_pt += 1;
                if (path_pt >= P_x.shape(0))
                {
                    path_pt = P_x.shape(0) - 1;
                }

                Tx = T_x(p);
                Ty = T_y(p);
                // std::cout << "============ [TimeCritic] ckpt4.8" << std::endl;
                // if ((*data.path_pts_valid)[path_pt])
                // {
                // std::cout << "============ [TimeCritic] ckpt4.9" << std::endl;
                dx = P_x(path_pt) - Tx;
                dy = P_y(path_pt) - Ty;
                // std::cout << "============ [TimeCritic] ckpt4.99" << std::endl;
                num_samples += 1.0f;

                dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(p));
                summed_path_cost += sqrtf(dx * dx + dy * dy + 2.0 * dyaw * dyaw);
                // summed_path_cost += sqrtf(dx * dx + dy * dy);
                // }
            }
            // std::cout << "============ [TimeCritic] ckpt5" << std::endl;
            if (num_samples > 0)
            {
                cost[t] = summed_path_cost / num_samples;
            }
            else
            {
                cost[t] = 0.0f;
            }
        }
        data.costs += xt::pow(std::move(cost) * weight_1_, power_);
        // std::cout << "============ [TimeCritic] ckpt6" << std::endl;
    }

And the controller parameters are:

  ros__parameters:
    controller_frequency: 30.0  # 控制器的更新频率
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"  # 指定使用 MPPI 控制器
      time_steps: 40  # 预测轨迹的时间步数
      model_dt: 0.05  # 每个时间步的持续时间（秒）
      batch_size: 3000  # 每次迭代中采样的轨迹数量
      vx_std: 0.3  # X轴速度的采样标准差
      vy_std: 0.3  # Y轴速度的采样标准差（仅限全向运动模型）
      wz_std: 0.7  # 角速度的采样标准差
      vx_max: 0.8  # 最大前进速度（米/秒）
      vx_min: -0.8  # 最大后退速度（米/秒）
      wz_max: 3.5  # 最大旋转速度（弧度/秒）
      iteration_count: 1  # MPPI 算法中的迭代次数
      enforce_path_inversion: false
      prune_distance: 2.4  # 路径修剪距离
      transform_tolerance: 0.1  # 数据变换的时间容忍度
      temperature: 0.01  # 轨迹成本的选择性（越接近0考虑的成本越低）
      gamma: 0.01  # 平滑性和低能量之间的权衡
      motion_model: "Ackermann"  # 使用的运动模型（DiffDrive, Omni, Ackermann）
      visualize: true  # 是否发布用于可视化的调试轨迹
      reset_period: 1.0  # 优化器重置所需的不活动时间
      regenerate_noises: false  # 是否在每次迭代中重新生成噪声
      AckermannConstraints:
        min_turning_r: 0.24
      # 以下是各种评价函数（critics）的配置
      critics: ["TimeCritic", "ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
      ConstraintCritic:
        cost_weight: 0.0
        cost_power: 1
      GoalCritic:
        cost_weight: 0.0
        cost_power: 1
        threshold_to_consider: 0.1
      GoalAngleCritic:
        cost_weight: 3.0
        cost_power: 1
        threshold_to_consider: 0.5
      PreferForwardCritic:
        cost_weight: 0.0 # 0.0
        cost_power: 1
        threshold_to_consider: 0.5
      ObstaclesCritic:
        consider_footprint: false
        collision_cost: 10000.0
        collision_margin_distance: 0.1
        inflation_radius: 0.55
        cost_scaling_factor: 10.0
      PathAlignCritic:
        cost_weight: 50.0 # 20.0
        cost_power: 1
        max_path_occupancy_ratio: 0.05
        threshold_to_consider: 0.1
        offset_from_furthest: 20
        use_path_orientations: true
      PathFollowCritic:
        cost_weight: 50.0 # 50.0
        cost_power: 1
        threshold_to_consider: 0.1
        offset_from_furthest: 20
      PathAngleCritic:
        cost_weight: 0.0 # 5.0
        cost_power: 1
        threshold_to_consider: 0.5
        forward_preference: false
      TimeCritic:
        path_dt: 0.1
        cost_weight: 0.0 # 5.0
        cost_weight_1: 50.0 # 100.0
        cost_weight_2: 0.0 # 10.0
        cost_power: 1

And I reupload the video, where the car zig-zag when follow the path.

Screencast.2024-03-28.14.17.19.mp4

[SteveMacenski]

This looks like just the path alignment critic -- what is substantively different about this from the current path align critic? Wouldn't you rather be looking at the velocities of the trajectories instead? if you have some sequence of poses in a path with a timestamp, you can find the velocity between them that enables that easily. Then the trajectories you have their velocities, so it should be an easy subtraction to find the difference and then sum those up to average for each trajectory sample 😄

Also, I'd recommend pulling in the latest changes I made (after 45% increase in performance PR), some of the utils that you're using were changed. I don't recall if I fixed any bugs in them, but I think at least one had a bug that was resolved (the rest are just faster now).