Add OSQP block #209

traversaro · 2021-05-18T14:51:09Z

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:

Example model:
qpoases_vs_osqp_test_v3.zip

traversaro · 2021-05-18T14:52:23Z

cc @CarlottaSartore

traversaro · 2021-05-18T15:06:03Z

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 · 2021-05-18T17:26:23Z

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:

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 · 2021-05-19T08:13:22Z

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 · 2021-05-19T08:30:56Z

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 · 2021-05-23T13:08:23Z

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 · 2021-05-23T20:46:17Z

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 · 2021-05-23T21:03:34Z

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 · 2021-05-23T21:06:49Z

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 · 2021-05-24T07:24:13Z

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 · 2021-05-24T07:31:56Z

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 :

diegoferigo

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 :)

diegoferigo · 2021-05-24T07:33:06Z

toolbox/library/src/OSQP.cpp

+    return true;
+}
+
+bool wbt::block::OSQP::solverInitialization(const BlockInformation*blockInfo)


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

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

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

toolbox/library/src/OSQP.cpp

diegoferigo · 2021-05-24T07:41:54Z

toolbox/library/src/OSQP.cpp

+    if (solveReturnVal) {
+        solution_colMajor = pImpl->sqSolver->getSolution();
+    }


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

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

Ok now I got the logic, makes sense!

traversaro · 2021-05-24T08:05:43Z

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

wb-toolbox/matlab/export_library.m

Line 23 in b776e49

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

.

gabrielenava · 2021-05-24T08:11:03Z

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.

traversaro · 2021-05-24T08:12:46Z

matlab/library/WBToolboxLibrary_repository.mdl

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


This seems dangerous.

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.

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.

It can definitely be added in the export script

traversaro · 2021-05-24T14:24:47Z

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.

Fix #180

traversaro · 2021-05-24T14:40:01Z

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 · 2021-05-24T14:46:41Z

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

gabrielenava · 2021-05-24T14:52:16Z

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 · 2021-05-24T15:12:49Z

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.

nunoguedelha · 2021-05-24T16:21:40Z

@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 · 2021-05-24T16:35:05Z

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 · 2021-05-24T19:31:29Z

@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 · 2023-12-05T13:27:48Z

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 · 2023-12-05T15:07:41Z

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 · 2023-12-06T02:12:30Z

你好@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.

traversaro mentioned this pull request May 19, 2021

Re-implement the Holder block to make it compatible with code generation and 'treat as atomic unit' #211

Merged

This was referenced May 19, 2021

Package CasADi MATLAB bindings robotology/robotology-superbuild#719

Closed

[WIP] QpOases: Switch to use reliable options #208

Closed

Clarify Data class methods semantics robotology/osqp-eigen#97

Closed

traversaro force-pushed the fix/180 branch from e9b61f7 to e6274af Compare May 23, 2021 18:09

traversaro force-pushed the fix/180 branch from e6274af to 09bd6e9 Compare May 23, 2021 20:54

traversaro changed the title ~~[WIP] First prototype of OSQP block~~ Add OSQP block May 23, 2021

traversaro requested review from nunoguedelha and CarlottaSartore May 23, 2021 21:03

traversaro mentioned this pull request May 23, 2021

WBToolbox: Explicitly enable dependency on ICUB, qpOASES and OsqpEigen robotology/robotology-superbuild#755

Merged

diegoferigo approved these changes May 24, 2021

View reviewed changes

diegoferigo mentioned this pull request May 24, 2021

How to use osqp in Simulink? osqp/osqp#113

Open

diegoferigo assigned traversaro May 24, 2021

diegoferigo added component::blocks component:simulink pr::status::feedback complexity::hard labels May 24, 2021

traversaro commented May 24, 2021

View reviewed changes

traversaro mentioned this pull request May 24, 2021

Re-run clang-format on source files #216

Open

Add OSQP block

7ffcef0

Fix #180

traversaro force-pushed the fix/180 branch from b8d2cdc to 7ffcef0 Compare May 24, 2021 14:38

traversaro merged commit ad4e8bd into master May 24, 2021

traversaro mentioned this pull request May 24, 2021

Clarification on contributing process #214

Open

Add OSQP block #209

Add OSQP block #209

Conversation

traversaro commented May 18, 2021 • edited Loading

traversaro commented May 18, 2021

traversaro commented May 18, 2021

traversaro commented May 18, 2021

traversaro commented May 19, 2021

traversaro commented May 19, 2021

traversaro commented May 23, 2021

traversaro commented May 23, 2021

traversaro commented May 23, 2021

traversaro commented May 23, 2021

diegoferigo commented May 24, 2021

traversaro commented May 24, 2021

diegoferigo left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

traversaro commented May 24, 2021 • edited Loading

gabrielenava commented May 24, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

traversaro commented May 24, 2021

traversaro commented May 24, 2021

traversaro commented May 24, 2021

gabrielenava commented May 24, 2021

traversaro commented May 24, 2021

nunoguedelha commented May 24, 2021

nunoguedelha commented May 24, 2021

traversaro commented May 24, 2021

EricWang0306 commented Dec 5, 2023

traversaro commented Dec 5, 2023

EricWang0306 commented Dec 6, 2023

traversaro commented May 18, 2021 •

edited

Loading

traversaro commented May 24, 2021 •

edited

Loading

gabrielenava commented May 24, 2021 •

edited

Loading