I want to know how to print some control effects.

[chan-yuu]

I want to know how to print some control effects, such as cte, epsi, etc.

[chan-yuu]

What I mean is, I can't find where they were calculated.

[TorBorve]

I am not sure if you can get these values directly. The ACADOS solver only returns the total cost I belive. Therefore, you have to calculate the cte and epsi yourself. You can do this by looking at how cte and epsi is caluculated in the Acados python file:

pathYaw = atan(3*coeffs[3]*x1*x1 + 2*coeffs[2]*x1 + coeffs[1])
epsi = psi - pathYaw
yPath = coeffs[3]*x1**3 + coeffs[2]*x1**2 + coeffs[1]*x1 + coeffs[0]
cte = yPath - y1

And then add the same code and print it in the MPCReturn PathTrackingSys::solve function. There you have access to the state and polynomial coefficients.

You can use LOG_DEBUG_STREAM and the uncomment

# Uncomment if you want debugging
#set(CMAKE_BUILD_TYPE Debug)

In CMakeLists.txt to get debug print. You can also print normally.

[chan-yuu]

Is that what you mean:

    pathYaw = atan(3*coeffs[3]*x1*x1 + 2*coeffs[2]*x1 + coeffs[1])
    epsi = psi - pathYaw
    yPath = coeffs[3]*x1**3 + coeffs[2]*x1**2 + coeffs[1]*x1 + coeffs[0]
    cte = yPath - y1
    print("cte:",cte, "epsi", epsi)
    print("!~~~~~~~~~~~~~~~~~!!!!!")

and the launch file is

    <node pkg="mpc_local_planner" type="ff_mpc" name="mpc" output="screen" >
        <!-- load parameters for the mpc -->
        <rosparam command="load" file="$(find mpc_local_planner)/build/auto_gen.yaml" />
    </node>

But only these contents were printed：

[TorBorve]

No, you can't print from python it has to be in the MPCReturn PathTrackingSys::solve function in the PathTrackingSys.cpp file. This is because how it works is as follows:

Acados is used to generate C-code for the MPC defined in the python file. When running the MPC the Cpp Ros package calls the generated C-functions by Acados. That is, the python script is only ran once to create C-code for the MPC. It is not run when you start the ROS node.

[chan-yuu]

MPCReturn PathTrackingSys::solve(const State &state, double pitch, double vRef, double loop_HZ, std::vector<double> &augmentedStates) {
    double rotation;
    Eigen::Vector4d coeffs;
    calcCoeffs(state, rotation, coeffs);
    Acados::PathTrackingParams params{coeffs, pitch, vRef};

    //compute gamma by approximation the integral with eulers method
    augmentedStates[0] = augmentedStates[0] + (vRef - (state.vel)) * (1 / loop_HZ);

    double pathYaw = atan(3 * coeffs[3] * state.x * state.x + 2 * coeffs[2] * state.x + coeffs[1]);
    double epsi = state.psi - pathYaw; 
    double yPath = coeffs[3] * std::pow(state.x, 3) + coeffs[2] * std::pow(state.x, 2) + coeffs[1] * state.x + coeffs[0]; // Path Y value
    double cte = yPath - state.y; 

I'm trying to calculate cte and epsi. Do you have any recommended formulas? I'm not sure if they're correct.

[TorBorve]

It looks correct. You can try to print it.

Personally is usually just use Rviz for tuning. There you can see where the car is relative to the path (cte) and also the angle epsi, visually. Furthermore, you can also see what the mpc is planning to do.

[chan-yuu]

Thank you for your help. I should have calculated it now. I have modified the PathTrackingSys.cpp file:

MPCReturn PathTrackingSys::solve(const State &state, double pitch, double vRef, double loop_HZ, std::vector<double> &augmentedStates) {
    double rotation;
    Eigen::Vector4d coeffs;
    calcCoeffs(state, rotation, coeffs);
    Acados::PathTrackingParams params{coeffs, pitch, vRef};

    // Step 1:
    double minDistance = std::numeric_limits<double>::max();
    int nearestIndex = -1;

    for (size_t i = 0; i < track_.size(); ++i) {
        double dx = track_[i].x - state.x;
        double dy = track_[i].y - state.y;
        double distance = std::sqrt(dx * dx + dy * dy);
        if (distance < minDistance) {
            minDistance = distance;
            nearestIndex = i;
        }
    }

    if (nearestIndex == -1) {
        ROS_WARN("Couldn't find a valid path point.");
        return MPCReturn{};
    }

    // Step 2: 
    int nextIndex = std::min(nearestIndex + 1, static_cast<int>(track_.size() - 1));
    double dxPath = track_[nextIndex].x - track_[nearestIndex].x;
    double dyPath = track_[nextIndex].y - track_[nearestIndex].y;

    // Step 3: 
    double dxVehicle = state.x - track_[nearestIndex].x;
    double dyVehicle = state.y - track_[nearestIndex].y;
    double cte = (dxVehicle * dyPath - dyVehicle * dxPath) / std::sqrt(dxPath * dxPath + dyPath * dyPath);

    // Step 4: 
    double pathYaw = atan2(dyPath, dxPath);
    double epsi = state.psi - pathYaw;

    // ensure EPSI in [-pi, pi] 
    while (epsi > M_PI) epsi -= 2 * M_PI;
    while (epsi < -M_PI) epsi += 2 * M_PI;

    ROS_INFO_STREAM("Cross Track Error (cte): " << cte);
    ROS_INFO_STREAM("Heading Error (epsi): " << epsi);

    State transformedState{0, 0, rotation, state.vel, augmentedStates[0], state.delta, state.throttle};

[chan-yuu]

# Distance between front and rear wheels on the car.
wheelbase: 1.77

# Wheel radius
radius: 0.225

# Gear ratio
gear_ratio: 0.05

# Mass of car
mass_car: 70

# Voltage on batteries
voltage: 48

# Drag coefficient
drag_coefficient: 0.22051

# Frontal area of the car
frontal_area: 0.766

# Rolling resistance
rolling_resistance: 0.015

Why do mpc have these parameters? What are Gear ratio and Frontal area of the car? I haven't seen it before.

[chan-yuu]

actual_steering_topic: "/steering_state" # [rad]?I think. Because its maximum value is around 10
steering_topic: "/steering_cmd"

By the way, these steering_state and steering_cmd are steering angles（Above 500 degrees） rather than wheel angles(33 degress), right?

[TorBorve]

The parameters you are referring to can be ignored. They are not used. It was supposed to make the mpc energy efficient, however it is not used anymore.

The values are the wheel_angles*steering_ratio. If you set steering ratio to 1, then they are the wheel angles