search

ethz-adrl / control-toolbox

The Control Toolbox - An Open-Source C++ Library for Robotics, Optimal and Model Predictive Control
BSD 2-Clause "Simplified" License
1.47k stars 314 forks source link

CARE results for high-dim systems seem to differ from the Matlab solution #119

Open GuiyangXin opened 4 years ago

GuiyangXin commented 4 years ago

Hi there,

I am trying to integrate CT with my program. But I got this error solveSchurDirect() in CARE can only be used if the lapack library is installed on your system when I am calling lqrSolver.compute(Q, R, A_ct, B_ct, K_ct). If I use lqrSolver.compute(Q, R, A_ct, B_ct, K_ct, false, true), I can got a resulted K_ct matrix. But I checked the example rosrun ct_optcon ex_LQR, it works without any problem. I think this rosrun ex_LQR implements the LQR.cpp in example folder. In that LQR.cpp, you use lqrSolver.compute(Q, R, A, B, K). I don't know why I can run lqrSolver.compute(Q, R, A, B, K) in that example without prompting an error: solveSchurDirect() in CARE can only be used if the lapack library is installed on your system

If i use lqrSolver.compute(Q, R, A_ct, B_ct, K_ct, false, true), it takes too long time to compute the K matrix of LQR. I will to try Lapack and Matlab. But I don't know how to set the macro #ifdef CT_USE_LAPACK. I never used this kind of macro in my code. If I want to set CT_USE_LAPACK to be true, should I do catkin build -DCT_USE_LAPACK=true ct_optcon.

Thanks in advance,

Guiyang

markusgft commented 4 years ago

Hi! Is lapack installed on your system? If not, you can install it by using sudo apt install liblapack-dev. Otherwise, it might be a linking issue. Are you linking your code appropriately against ct_optcon? If yes, it may help to set the compile definition explicitly, you could try catkin build -DCT_USE_LAPACK=true

GuiyangXin commented 4 years ago

Thanks for your answer. I downloaded lapack source and installed it via make install. After running sudo apt install liblapack-dev, I got this liblapack-dev is already the newest version (3.7.1-4ubuntu1). I also tried to compile ct_optcon using catkin build -DCT_USE_LAPACK=true ct_optcon. But I still got "solveSchurDirect() in CARE can only be used if the lapack library is installed on your system" when calling lqrSolver.compute(Q, R, A, B, K) in my cpp file. P.S., I defined Q, R, A, B, K as Eigen matrices instead of ct types. Does it matter?

Cheers,

Guiyang

markusgft commented 4 years ago

Is your code located in a separate repository? In that case, you need to compile your code with the flag CT_USE_LAPACK, too. And link against ct_optcon in target_link_libraries(... ct_optcon) in your code. The type of matrix does not matter.

GuiyangXin commented 4 years ago

My catkin package and CT are in the same workspace.

GuiyangXin commented 4 years ago

Can I try Matlab by setting -DMATLAB=true?

markusgft commented 4 years ago

My catkin package and CT are in the same workspace.

That is fine, but it was not the intent of my question. Please refer to my above comments.

GuiyangXin commented 4 years ago

In my hpp file, I declare the object as ct::optcon::LQR<36, 18> lqrSolver; In cpp file, I use it as lqrSolver.compute(Q, R, A_ct, B_ct, K_ct);

markusgft commented 4 years ago

the control toolbox, i.e. ct_optcon, is not a catkin package! So you cannot add it to CATKIN_DEPENDS, etc. You need to treat it like a pure cmake library. In catkin_package, you should add it to the DEPENDS tag, and when linking, you should write

target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES} ct_optcon
)
markusgft commented 4 years ago

@GuiyangXin has this been resolved now?

GuiyangXin commented 4 years ago

I am still working on it. I will let you know. Thanks.

GuiyangXin commented 4 years ago

Hi @markusgft,

The problem is solved. You are right. CT is not a catkin package. My cmakelist was wrong. Also, I think using lapack is the default method for lqr.compute(). Using Lapack to solve Riccati equation is much faster than the iterative method. But the K gain from lqr.compute() doesn't look like correct. I need to test it and compare it with Matlab.

Thanks a lot,

Guiyang

GuiyangXin commented 4 years ago

Hi @markusgft,

I tested lqr.compute() in different cases. I compiled the code with -DCT_USELAPACK=true. I first tested it with a very simple example, where A = [-0.313 56.7 0; -0.0139 -0.426 0; 0 56.7 0]; B = [0.232; 0.0203; 0]; In this case, co = ctrb(A,B) and rank(co)=3, which means the system is fully controlled. lqr.compute(A,B,Q,R,K) got the same result as the one in Matlab. However, in my application case of quadruped control, lqr.compute() of CT cannot get the same result as in Matlab. Actually, the result of lqr.compute() doesn't make sense. Some elements of the result K matrix are extremely large. The Matlab result is quite good as I can control the robot standing still using the tau=-K{Matlab}(x-x_d). I think it is probably due to the large dimension of the system. ct::optcon::LQR<36, 18> lqrSolver; Eigen::Matrix<double, 18, 36> K; I also checked the rank of controllability matrix using Matlab. It is 16 that is less than the state number 36. Is it the reason for why CT cannot generate the correct result (or the same result as Matlab)?

markusgft commented 4 years ago

Dear @GuiyangXin, thank you for reporting the issue. It might well be that there are some numerical issues hidden somewhere, let us try to find them! In order to find the problem, we need to try to narrow down the source of the error.

  1. How are you computing the continuous-time linearization of your robot model? Is that done with a CT linearizer, or are you implementing the linearized matrices directly?
  2. Are you using a diagonal R matrix? If yes, could you please verify if setting the option "R_is_diagonal" changes anything?
  3. Are you using the iterative or the direct Schur method? Do they give you different results? Best
GuiyangXin commented 4 years ago

Hi @markusgft ，

  1. I am doing numerical finite differentiation manually. After getting the linearised system (A and B matrices), I call lqr.compute(Q,R,A,B,K).
  2. The R matrix is diagonal in my case. I tried lqr.compute(Q,R,A,B,K, false) and lqr.compute(Q,R,A,B,K, true). They gave me different result. But both of them are not close to Matlab result. And they are obviously wrong. For example the 7th row of K is [-4.14462 4.08439e+07 -1.89291e+07 91771.4 2.52327e+07 -459529 7.49148e+06 1.41711e+06 -3.42112e+07 222035 7.1107e+06 223646 -1.06661e+07 -86225.1 374908 135944 -231754 -850607 48.8947 -56.8763 507.757 -135.933 154.925 29.6651 1.12489 -0.213368 -0.14311 2.00396 -0.183511 0.27159 -0.902749 0.237621 0.422135 -76.4815 -18.248 70.4138] By contrast, the 7th row of Matlab's result is [1.03915037078243 0.489656630985102 -0.573640396764342 3.98404408064149 2.54419772487326 -1.61618835050240 -0.964685645663186 202.206955602836 151.650508526794 43.7275144732421 154.203683701492 61.1488538591843 85.8831326192118 -53.3095619673432 -23.4274781757700 -26.8427831772922 -272.130006868088 -178.585107437120 -1.95409449390391 4.94404999159892 -38.3008708136983 -1.86572868205445 -5.18214353097519 -6.23820369808679 -2.07614541116709 26.0559364828927 20.0494620724432 -0.251480386581347 27.6416099860433 11.6546064842757 23.6228829110879 -12.1320133182468 -8.96777452716433 -12.1319288879070 -39.2813640546156 -23.6023620681006]
  3. If I use lqr.compute(Q,R,A,B,K, true,true), the computation will be too slow. It takes few seconds to give result. And some elements of the result are "nan". For example, the 7th row of K from lqr.compute(Q,R,A,B,K, true,true) is [-3.16107 0 0 0 0 0 0 0 25.438 -19.8207 78.1834 9.99697 -15.897 213.517 -22.9735 -59.5489 149.161 -28.8566 -4.4026 0 0 0 0 0 0 0 9.96145 0.923987 -19.5827 37.6283 -9.24979 -nan -nan -nan -971.224 149.909]. The result of lqr.compute(Q,R,A,B,K, true,false) doesn't have "nan".
markusgft commented 4 years ago

@GuiyangXin, this is getting more and more interesting. The CARE implementation is in fact unit-tested against the Matlab solution, however it might well be that there unknown problems for high-dim systems. There is a unit test for a system with intermediate dimensions, it considers a quadrotor, see here. Would you mind verifying that this unit test passes for you and that the matlab and CT results are identical? Some more related questions: which compiler are you using? Are you compiling with special optimization flags? Are you compiling in Release or debug?

GuiyangXin commented 4 years ago

Hi @markusgft ,

I tested the quadrotor example. Matlab and CARE got the same result. I think this is because the rank of controllability matrix is full rank. I used matlab to compute the rank of the controllability matrix. The rank is 12 that is equal to the state dimension. In my case, the state number 36 is much larger than the rank of controllability matrix (the rank is 16). But Matlab can give me a correct K matrix since I have used that K matrix successfully control the quadruped robot standing still. I think calling matlab function is not fast enough for real time control. I still want to use your CARE method. Do you have any idea?

markusgft commented 4 years ago

The Matlab documentation says that their CARE solver requires the pair (A,B) to be stabilizable. I will need to check back on the assumptions of our CARE solver implementation, it might well be that we need the pair (A,B) to be controllable. But I need to look up the details!

GuiyangXin commented 4 years ago

Hi @markusgft,

I want to use 

template <size_t STATE_DIM, size_t CONTROL_DIM>
bool LQR<STATE_DIM, CONTROL_DIM>::computeMatlab(const state_matrix_t& Q,
    const control_matrix_t& R,
    const state_matrix_t& A,
    const control_gain_matrix_t& B,
    control_feedback_t& K,
    bool checkControllability)
{
    if (!matlabEngine_.isInitialized())
        matlabEngine_.initialize();

    matlabEngine_.put("Q", Q);
    matlabEngine_.put("R", R);
    matlabEngine_.put("A", A);
    matlabEngine_.put("B", B);

    if (checkControllability)
    {
        matlabEngine_.executeCommand("Co=ctrb(A,B);");
        matlabEngine_.executeCommand("unco=length(A)-rank(Co);");
        int uncontrollableStates = 0;
        matlabEngine_.get("unco", uncontrollableStates);

        if (uncontrollableStates > 0)
        {
            std::cout << "System is not controllable, # uncontrollable states: " << uncontrollableStates << std::endl;
        }
    }

    matlabEngine_.executeCommand("[K,~,~] = lqr(A,B,Q,R);");

    Eigen::MatrixXd Kmatlab;
    matlabEngine_.get("K", Kmatlab);

    K = Kmatlab;

    return true;
}

But I cannot find this matlabCppInterface/Engine.hpp. What is this package matlabCppInterface?

markusgft commented 4 years ago

Hi, the matlabCppInterface package is currently not open-source, it is closed-source, belongs to ETHZ and will most likely not be released any time soon. So unfortunately, we cannot provide support for this feature. In the end, however, this is just a simple wrapper package which access the C/C++ API of Matlab.

GuiyangXin commented 4 years ago

All right. Thank you. I will try matlab API.

markusgft commented 4 years ago

See also the discussion in #130

markusgft commented 4 years ago

Here is a preliminary release of the package which holds the interface to matlab. https://github.com/ethz-adrl/matlab_cpp_interface

GuiyangXin commented 4 years ago

Hi @markusgft, Thank you very much for sharing the package. After compiling, I tried to run ./matlabTest. It doesn't work. 

Starting matlab engine test
Performing initialization test
matlabTest: /home/anymal/matlab_cpp_interface/test/MatlabInterfaceTests.hpp:9: void testInit(): Assertion `engine.initialize() && "initialization error"' failed.
Aborted (core dumped)

I don't know what the reason is. Maybe it is due to Matlab version, or installation. I will write a test file based on Matlab official tutorial.