NLP calculation speed is too slow

[sm3304love]

Hello. I'm currently trying to solve the manipulator's motion planning with NMPC using the libmpc library. This function was previously implemented using the casADi library, but due to a problem with casADi that external robotics libraries such as Pinocchio cannot be used, So I want to use libmpc.

The NMPC problem I'm currently trying to implement is shown in the image below.

I succeeded in implementing the NMPC problem with libmpc, but the problem occurred that the calculation time was too long. If the calculation time is short, it takes 1 second, and if it is long, it takes up to 5 seconds. Could you please advise on any methods I might try to accelerate NMPC calculations?

The code below is the objective function I set and The horizon used in the calculation is 10.

mpc_solver.setObjectiveFunction([&](const mpc::mat<pred_hor + 1, num_states> &x,
                                        const mpc::mat<pred_hor + 1, num_outputs> &,
                                        const mpc::mat<pred_hor + 1, num_inputs> &u, const double &) {
        // std::cout << "x" << std::endl << x << std::endl;
        double cost = 0;
        for (int i = 0; i < pred_hor; i++)
        {
            Eigen::VectorXd q = x.row(i).head(6);
            Eigen::VectorXd q_dot = x.row(i).tail(6);

            pinocchio::framesForwardKinematics(model, data, q);
            const pinocchio::SE3 &effector_pose = data.oMf[frame_id];
            Eigen::Quaterniond ee_ori(effector_pose.rotation());
            Eigen::Vector3d ee_pos = effector_pose.translation();

            Eigen::VectorXd pose_error = ee_pos - ee_pos_ref;
            Eigen::VectorXd ori_error = (ee_ori_ref * ee_ori.inverse()).vec();
            double pose_cost = pose_error.transpose() * Q_trans * pose_error;
            double ori_cost = ori_error.transpose() * Q_ori * ori_error;
            double vel_cost = q_dot.transpose() * Q_vel * q_dot;
            double input_cost = u.row(i).dot(R * u.row(i).transpose());

            cost += (pose_cost + ori_cost + vel_cost + input_cost);
        }

        // // final terminal cost
        Eigen::VectorXd q_final = x.row(pred_hor).head(6);

        pinocchio::framesForwardKinematics(model, data, q_final);
        const pinocchio::SE3 &effector_pose_final = data.oMf[frame_id];

        Eigen::Quaterniond ee_ori_final(effector_pose_final.rotation());
        Eigen::Vector3d ee_pos_final = effector_pose_final.translation();

        Eigen::VectorXd pose_error_final = ee_pos_final - ee_pos_ref;
        Eigen::VectorXd ori_error_final = (ee_ori_ref * ee_ori_final.inverse()).vec();

        cost += pose_error_final.dot(Qf_trans * pose_error_final) + ori_error_final.dot(Qf_ori * ori_error_final);
        return cost;
    });

If you want, you can check my entire algorithm through the link below. Your assistance is greatly appreciated. nonlinear_mpc

[nicolapiccinelli]

Hi, @sm3304love I took a look at the code, and from my side looks correct. Can you get the execution statistics using getExecutionStats() to check the average return state of the solver? I will be back in my office tomorrow, so I can try to run your repo on my PC. Are you using ROS 1 right?

[sm3304love]

Yes, I'm using ROS 1(noetic). I'll check execution statistics using getExecutionStats() and share the results.

[sm3304love]

As a result of checking the solution states using getExecutionStats(), I was able to confirm the following results.

[nicolapiccinelli]

@sm3304love in your repo ur20.urdf is missing, i cannot reproduce your setup.

[rospack] Error: package 'ur20_description' not found
[librospack]: error while executing command
Error:   File /urdf/ur20.urdf does not exist
         at line 55 in /build/urdfdom-VnCcob/urdfdom-1.0.4+ds/urdf_parser/src/model.cpp
terminate called after throwing an instance of 'std::invalid_argument'
  what():  The file /urdf/ur20.urdf does not contain a valid URDF model.
Annullato

[sm3304love]

@nicolapiccinelli I'm sorry. ur20.urdf is in this package. https://github.com/sm3304love/ur20_description

[nicolapiccinelli]

@sm3304love On my desktop, it's faster, but ofc not as casADi

but if you enable the compiler optimization flag -O3 like that set(CMAKE_CXX_FLAGS "-DEIGEN_STACK_ALLOCATION_LIMIT=0 -O3")

[sm3304love]

I followed your advice and set the compiler optimization flag, and my code became much faster. Thanks for your help!