Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add OSQP block #209

Merged
merged 1 commit into from
May 24, 2021
Merged

Add OSQP block #209

merged 1 commit into from
May 24, 2021

Conversation

traversaro
Copy link
Member

@traversaro traversaro commented May 18, 2021

This PR add an OSQP block based on osqp-eigen to fix #180 . It requires OsqpEigen to be installed and the WBT_USES_OSQPEIGEN option to be set.

Known problems:

  • KP1: The library is not visualized anymore in the Simulink Library Browser

Solved problems:

  • KP2: The UseLb and UseUb options are not supported
  • KP3: Setting only one of UseLba and UseUba to true while the other to false, gives the wrong results
  • KP4: All the use of the bfError macro are commented out because their use result in a compilation error
  • KP5: The ComputeObjVal, StopWhenFails` options are not supported
  • KP6: Non-deterministic "[OsqpEigen::Solver::initSolver] Unable to setup the workspace." error during initialization

Basic Tests again qpOASES block:
sqp_comparison

Example model:
qpoases_vs_osqp_test_v3.zip

Sorry, something went wrong.

@traversaro
Copy link
Member Author

cc @CarlottaSartore

@traversaro
Copy link
Member Author

Per alignment with @CarlottaSartore, the issue that needs to be solved before we can even try to test this in the relevant codebase is KP2 .

@traversaro
Copy link
Member Author

I just pushed a second version, the main differences are that the following KP have been solved:

  • KP2: The UseLb and UseUb options are not supported
  • KP3: Setting only one of UseLba and UseUba to true while the other to false, gives the wrong results
  • KP4: All the use of the bfError macro are commented out because their use result in a compilation error

So, the block should be ready to be tested. An example model is:
osqp_vs_qpoases_v2

qpoases_vs_osqp_test_v2.zip

However, a new known problem (KP) appeared, KP6: while the simulation starts fine most of the times, sometime during the initialization it fails with the following error:

[OsqpEigen::Solver::initSolver] Unable to setup the workspace.
An error occurred while running the simulation and the simulation was terminated
Caused by:
Error reported by S-function 'BlockFactory' in 'qpoases_vs_osqp_test/OSQP': 
Component: Simulink | Category: Block error

However, it seems that once the setup works fine, everything is able to work as expected, so for preliminar testing is should be ok.

@traversaro
Copy link
Member Author

I think I found the issue behind KP6 . As the problem was happening at initSolver time, I added a few prints there:

void wbt_print_csc_matrix(csc *M, const char *name)
{
  c_int j, i, row_start, row_stop;
  c_int k = 0;

  // Print name
  fprintf(stderr, "%s :\n", name);

  for (j = 0; j < M->n; j++) {
    row_start = M->p[j];
    row_stop  = M->p[j + 1];

    if (row_start == row_stop) continue;
    else {
      for (i = row_start; i < row_stop; i++) {
        fprintf(stderr, "\t[%3u,%3u] = %.3g\n", (int)M->i[i], (int)j, M->x[k++]);
      }
    }
  }
}
        OSQPSettings* settings = pImpl->sqSolver->settings()->getSettings();
        std::cerr << "OSQPSettings " << std::endl;
        std::cerr << "rho " << settings->rho << std::endl;
        std::cerr << "sigma " << settings->sigma << std::endl;
        std::cerr << "scaling " << settings->scaling << std::endl;
        std::cerr << "adaptive_rho " << settings->adaptive_rho << std::endl;
        std::cerr << "adaptive_rho_interval " << settings->adaptive_rho_interval << std::endl;
        std::cerr << "adaptive_rho_tolerance " << settings->adaptive_rho_tolerance << std::endl;
        std::cerr << "adaptive_rho_fraction " << settings->adaptive_rho_fraction << std::endl;
        std::cerr << "max_iter " << settings->max_iter << std::endl;
        std::cerr << "eps_abs " << settings->eps_abs << std::endl;
        std::cerr << "eps_rel " << settings->eps_rel << std::endl;
        std::cerr << "eps_prim_inf " << settings->eps_prim_inf << std::endl;
        std::cerr << "eps_dual_inf " << settings->eps_dual_inf << std::endl;
        std::cerr << "alpha " << settings->alpha << std::endl;
        std::cerr << "linsys_solver " << settings->linsys_solver << std::endl;
        std::cerr << "delta " << settings->delta << std::endl;
        std::cerr << "polish " << settings->polish << std::endl;
        std::cerr << "polish_refine_iter " << settings->polish_refine_iter << std::endl;
        std::cerr << "verbose " << settings->verbose << std::endl;
        std::cerr << "scaled_termination " << settings->scaled_termination << std::endl;
        std::cerr << "check_termination " << settings->check_termination << std::endl;
        std::cerr << "warm_start " << settings->warm_start << std::endl;
        std::cerr << "time_limit " << settings->time_limit << std::endl;

        OSQPData* data = pImpl->sqSolver->data()->getData();
        std::cerr << "OSQPData " << std::endl;
        std::cerr << "n " << data->n << std::endl;
        std::cerr << "m " <<  data->m << std::endl;
        std::cerr << "q ";
        for(int i=0; i <  data->n; i++) { std::cerr << data->q[i] << " "; }
        std::cerr << std::endl;
        std::cerr << "l ";
        for(int i=0; i < data->m; i++) { std::cerr << data->l[i] << " "; }
        std::cerr << std::endl;
        std::cerr << "u ";
        for(int i=0; i < data->m; i++) { std::cerr << data->l[i] << " "; }
        std::cerr << std::endl;
        std::cerr << "P " << std::endl;
        wbt_print_csc_matrix(data->P, "P");
        std::cerr << "A " << std::endl;
        wbt_print_csc_matrix(data->A, "A");

        if(!pImpl->sqSolver->initSolver()) {
            bfError << "OSQP: initSolver() failed.";
            return false;
        } else {
           std::cerr << "[OsqpEigen::Solver::initSolver] Workspace setup correctly." << std::endl;
        }

While the print of P and A are not working, I already noticed different numbers, that apparently influenced the success of the startup:
Working examples:

OSQPSettings 
rho 0.1
sigma 1e-06
scaling 10
adaptive_rho 0
adaptive_rho_interval 0
adaptive_rho_tolerance 5
adaptive_rho_fraction 0.4
max_iter 4000
eps_abs 0.001
eps_rel 0.001
eps_prim_inf 0.0001
eps_dual_inf 0.0001
alpha 1.6
linsys_solver 0
delta 1e-06
polish 0
polish_refine_iter 3
verbose 1
scaled_termination 0
check_termination 25
warm_start 1
time_limit 0
OSQPData 
n 2
m 4
q -10 20 
l 6.95183e-310 2.122e-314 4.94066e-324 4.24399e-314 
u 6.95183e-310 2.122e-314 4.94066e-324 4.24399e-314 
P 
A 
[OsqpEigen::Solver::initSolver] Workspace setup correctly.
OSQPSettings 
rho 0.1
sigma 1e-06
scaling 10
adaptive_rho 0
adaptive_rho_interval 0
adaptive_rho_tolerance 5
adaptive_rho_fraction 0.4
max_iter 4000
eps_abs 0.001
eps_rel 0.001
eps_prim_inf 0.0001
eps_dual_inf 0.0001
alpha 1.6
linsys_solver 0
delta 1e-06
polish 0
polish_refine_iter 3
verbose 1
scaled_termination 0
check_termination 25
warm_start 1
time_limit 0
OSQPData 
n 2
m 4
q -10 20 
l 6.95183e-310 2.122e-314 9.88131e-324 4.24399e-314 
u 6.95183e-310 2.122e-314 9.88131e-324 4.24399e-314 
P 
A 

Not working:

OSQPSettings 
rho 0.1
sigma 1e-06
scaling 10
adaptive_rho 0
adaptive_rho_interval 0
adaptive_rho_tolerance 5
adaptive_rho_fraction 0.4
max_iter 4000
eps_abs 0.001
eps_rel 0.001
eps_prim_inf 0.0001
eps_dual_inf 0.0001
alpha 1.6
linsys_solver 0
delta 1e-06
polish 0
polish_refine_iter 3
verbose 1
scaled_termination 0
check_termination 25
warm_start 1
time_limit 0
OSQPData 
n 2
m 4
q -10 20 
l 6.76115e+223 7.8423e+198 7.07376e+194 1.16498e-311 
u 6.76115e+223 7.8423e+198 7.07376e+194 1.16498e-311 
P 
A 
[OsqpEigen::Solver::initSolver] Workspace setup correctly.
[OsqpEigen::Solver::solve] The solution is unfeasible.
OSQPSettings 
rho 0.1
sigma 1e-06
scaling 10
adaptive_rho 0
adaptive_rho_interval 0
adaptive_rho_tolerance 5
adaptive_rho_fraction 0.4
max_iter 4000
eps_abs 0.001
eps_rel 0.001
eps_prim_inf 0.0001
eps_dual_inf 0.0001
alpha 1.6
linsys_solver 0
delta 1e-06
polish 0
polish_refine_iter 3
verbose 1
scaled_termination 0
check_termination 25
warm_start 1
time_limit 0
OSQPData 
n 2
m 4
q -10 20 
l 6.95183e-310 2.122e-314 9.88131e-324 4.24399e-314 
u 6.95183e-310 2.122e-314 9.88131e-324 4.24399e-314 
P 
A 
[OsqpEigen::Solver::initSolver] Unable to setup the workspace.

@traversaro
Copy link
Member Author

Commit e9b61f7 solved KP6, by ensuring that the Eigen vector that contain the buffers do not go out of scope for the duration of OSQP Solver life, or at least until updateBounds is called again. The root issue may lay in the osqp-eigen interface docs, and I will open a separate issue for that.

@traversaro
Copy link
Member Author

The root issue may lay in the osqp-eigen interface docs, and I will open a separate issue for that.

I did that, but actually the docs were there but I did not read them: robotology/osqp-eigen#97 .

@traversaro
Copy link
Member Author

I fixed the KP5, implementing the support for ComputeObjVal and StopWhenFails options, and I rebased the PR on top of latest master, after PRs #211 and #212 .

@CarlottaSartore if you need the version from past week (perhaps because this improved version has some regression) you can find it in https://github.com/robotology/wb-toolbox/tree/fix_180_first_prototype .

@traversaro traversaro changed the title [WIP] First prototype of OSQP block Add OSQP block May 23, 2021
@traversaro
Copy link
Member Author

The PR is now ready for review. The only remaining problems seems to be "KP1: The library is not visualized anymore in the Simulink Library Browser". However, it seems that there is the same problem on master after #211, do you have any idea on what could be causing this or on how to debug this @nunoguedelha @diegoferigo @Giulero @gabrielenava ?

@traversaro
Copy link
Member Author

The Codacy failure is a false positive, the error is:

Variable 'solution_colMajor' is assigned a value that is never used.

But solution_colMajor is a Eigen::Map object, so it is perfectly fine to assign it and never reference it again. @diegoferigo I would be inclined to disable Codacy, do you have any better suggestion?

@diegoferigo
Copy link
Member

The Codacy failure is a false positive, the error is:

Variable 'solution_colMajor' is assigned a value that is never used.

But solution_colMajor is a Eigen::Map object, so it is perfectly fine to assign it and never reference it again. @diegoferigo I would be inclined to disable Codacy, do you have any better suggestion?

Do you have access to Codacy? This is a common problem for Eigen maps. When false positive like this one occur, the error can be manually ignored directly from Codacy.

@traversaro
Copy link
Member Author

The Codacy failure is a false positive, the error is:

Variable 'solution_colMajor' is assigned a value that is never used.

But solution_colMajor is a Eigen::Map object, so it is perfectly fine to assign it and never reference it again. @diegoferigo I would be inclined to disable Codacy, do you have any better suggestion?

Do you have access to Codacy? This is a common problem for Eigen maps. When false positive like this one occur, the error can be manually ignored directly from Codacy.

Apparently I can't :
codacy_github

Copy link
Member

@diegoferigo diegoferigo left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nicely done @traversaro, this PR looks really good! Apart from some minor nits, I really need to check on how to add a workflow that enforces clang-format on our projects :)

return true;
}

bool wbt::block::OSQP::solverInitialization(const BlockInformation*blockInfo)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Missing space in parameter. Did your IDE pick clang-format?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

To be honest, I have to check how to do that on Windows, and who can provide clang-format there.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think that clang-tools in conda-forge should have clang-format also on Windows.

toolbox/library/src/OSQP.cpp Outdated Show resolved Hide resolved
Comment on lines +558 to +559
if (solveReturnVal) {
solution_colMajor = pImpl->sqSolver->getSolution();
}
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe from here on we can assume that solveReturnVal is true, right? This if seems redundant.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No, if stopWhenFails is false you don't know if solveReturnVal is true or false.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok now I got the logic, makes sense!

@traversaro
Copy link
Member Author

traversaro commented May 24, 2021

The PR is now ready for review. The only remaining problems seems to be "KP1: The library is not visualized anymore in the Simulink Library Browser". However, it seems that there is the same problem on master after #211, do you have any idea on what could be causing this or on how to debug this @nunoguedelha @diegoferigo @Giulero @gabrielenava ?

I think the issue is this one:

In the MATLAB® Command Window, enter this command to set the EnableLBRepository library property to 'on'. Your library will appear in the Library Browser only if this property is on when you save your library.
set_param(gcs,'EnableLBRepository','on');

From https://www.mathworks.com/help/simulink/ug/adding-libraries-to-the-library-browser.html .

Even if it is done in

set_param(libraryName, 'EnableLBRepository', 'on');
.

@gabrielenava
Copy link
Collaborator

gabrielenava commented May 24, 2021

The PR is now ready for review. The only remaining problems seems to be "KP1: The library is not visualized anymore in the Simulink Library Browser". However, it seems that there is the same problem on master after #211, do you have any idea on what could be causing this or on how to debug this @nunoguedelha @diegoferigo @Giulero @gabrielenava ?

on iRonCub we had sometimes issues related to our Simulink library not visualizing correctly in the library browser. I don't know if it can help, but the most common causes were:

  • installation path not linked correctly in Matlab;
  • refresh the library browser, sometimes it does not update until refresh;
  • some blocks were designed with a newer version of Simulink;
  • we have a main library block, connected with several sublibraries. The sublibrary was not linked correctly in the main library block;
  • the script slblocks.m does not link correctly to the associated library model.
  • there may be conflicts if the installation path changes and Simulink detects multiple models with the same name. Cleaning the install folder and reinstall from scratch sometimes helped.

@@ -1,7 +1,7 @@
Library {
Name "WBToolboxLibrary_repository"
Version 8.4
SavedCharacterEncoding "UTF-8"
SavedCharacterEncoding "windows-1252"
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This seems dangerous.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yep. One thing I remember I was doing is, when possible, picking up only the chunks of the mdl that were directly related to the newly added / modified block (quite difficult from the command line, surprisingly easy from tools like GitKraken). I'm not sure if this could be done every time due to the re-numbering of the IDs.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This was fixed by calling slCharacterEncoding('UTF-8') in MATLAB. In general while editing the library running:

set_param(gcs,'EnableLBRepository','on');
slCharacterEncoding('UTF-8')

is beneficial.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It can definitely be added in the export script

@traversaro
Copy link
Member Author

on iRonCub we had sometimes issues related to our Simulink library not visualizing correctly in the library browser. I don't know if it can help, but the most common causes were:

Thanks a lot, this will help. A bit of in-line comments

installation path not linked correctly in Matlab;

By "not linked correctly" you mean that it was not in the directories that you obtain by running path, due to typos or similar?

refresh the library browser, sometimes it does not update until refresh;

For reference, all the tests for this PR were done by refreshing the library browser every time.

some blocks were designed with a newer version of Simulink;

In WB-Toolbox case, there is only one library, so i think that should not be a problem as all the blocks are saved in the same library?

we have a main library block, connected with several sublibraries. The sublibrary was not linked correctly in the main library block;

This is not relevant in this case as we only have one lib, but what do you mean in this context with "not linked correctly"?

the script slblocks.m does not link correctly to the associated library model.

"do not link correctly" in this context means that it has the wrong name?

there may be conflicts if the installation path changes and Simulink detects multiple models with the same name. Cleaning the install folder and reinstall from scratch sometimes helped.

Thanks. For reference, all the tests I reported were done on a clean run of Simulink and by deleting the install folder.

@traversaro
Copy link
Member Author

I am not sure what happened, but after all the tests it seems that the version now on the PR works correctly and can be found by the Library Browser, so the PR is ready for merge.

@traversaro
Copy link
Member Author

@CarlottaSartore when you want I can proceed with the merge and then I can prepare the v5.4.0 release.

@gabrielenava
Copy link
Collaborator

I am not sure what happened, but after all the tests it seems that the version now on the PR works correctly and can be found by the Library Browser, so the PR is ready for merge.

It's the magic of Simulink (cc @Giulero)

@traversaro
Copy link
Member Author

I am not sure what happened, but after all the tests it seems that the version now on the PR works correctly and can be found by the Library Browser, so the PR is ready for merge.

It's the magic of Simulink (cc @Giulero)

My hypothesis is that I was missing some steps, this was the first time that I tried systematically, uninstalling every time, etc etc.

@traversaro traversaro merged commit ad4e8bd into master May 24, 2021
@nunoguedelha
Copy link
Collaborator

@traversaro , I didn't have a chance to review the last version. It's ok because there are two other reviewers, but actually the model file .../robotology-superbuild/src/WBToolbox/matlab/library/WBToolboxLibrary_repository.mdl is still the version 2021b and I would have noticed that before the merge 😄 . Could you please convert it to 2019b? I don't know if you want to apply the policy you mentioned recently (support the last 4 MATLAB versions)?

@nunoguedelha
Copy link
Collaborator

I would have noticed that before the merge

What I mean is that I might be the only one still using MATLAB 2020b apparently. I'm always a bit fearful of the latest versions 😓 .

@traversaro
Copy link
Member Author

@traversaro , I didn't have a chance to review the last version. It's ok because there are two other reviewers, but actually the model file .../robotology-superbuild/src/WBToolbox/matlab/library/WBToolboxLibrary_repository.mdl is still the version 2021b and I would have noticed that before the merge 😄 . Could you please convert it to 2019b? I don't know if you want to apply the policy you mentioned recently (support the last 4 MATLAB versions)?

Sure, but I guess this is related to the discussion in: #214 . I will comment there.

@EricWang0306
Copy link

Hello there:
sorry for my English;
I just install the WBToolbox and added it into the Matlab toolbox. Then I download this example '[qpoases_vs_osqp_test_v3.zip]' and try to run it to check the OSQP solver. But I just failed to run it for an error showed that : S-Function 'BlockFactory' does not exist. I don't know how to solve the problem and I downloaded the 'https://github.com/robotology/blockfactory.git' code and put it into the MATLAB working path. But it didn't work. The error still there. Could you please tell me how to correct the error?
Am I doing their installation methods wrong? Or it's because I didn't install osqp-eigen.
Looking forward your reply.

@traversaro
Copy link
Member Author

Hello @EricWang0306 , can you open a new issue in https://github.com/robotology/wb-toolbox/issues/new/choose explaining how did you installed the wb-toolbox? Thanks!

@EricWang0306
Copy link

你好@EricWang0306,您可以在https://github.com/robotology/wb-toolbox/issues/new/choose中打开一个新问题来解释您是如何安装 wb-toolbox 的吗?谢谢!

I'm sorry to reply an old issue. I've open an new issue in #241. Looking forward your reply.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

Add OSQP block to deprecate the QpOases block
5 participants