Failure when two QP blocks - one with inequalities and the other not - are used in the same model

[gabrielenava]

• I already updated to the latest version I can use
• I already checked similar issues using the search box
• I already checked the website for known troubleshooting
• I already cleaned my environment (by removing the wb-toolbox install folder and installing it again)
• I already excluded the possibility that system configuration files (e.g. yarp related) might be responsible
• I am sure the environment variables are correct
• I tried to reproduce the error in simulation (if it affects the real robot)

Description

When two QP blocks are added to the same Simulink model, and one of the two uses both input boundaries and inequality constraints while the other uses only input boundaries, the model exits with an error right after startup.

Platform

iRonCub controller, simple Simulink example (attached to this issue).

Reproducibility

Here is a simple Simulink model I created to reproduce the bug:

testQPblock.zip

Screenshots

Here is an overview of the simple Simulink model

Here is the error

the error does not appear if I run only 1 QP and I comment the other, or if I run QPTest 1 (unconstrained) and QPTest 3 or QPTest 1 and QPTest 2.

Additional information

• OS: Ubuntu 18.04 LTS
• Version / Commit: 0688771
• Matlab Version: 2019b

[diegoferigo]

Thanks @gabrielenava for filling this issue. The qpOASES block has quite a convoluted logic for its port due to its high number of signal. Things are even more complicated since all its signals are dynamically sized.

On the fly, I cannot explain why the blocks do not work if they are both enabled in the model. I suspect that a 1D signal is treated as a vector instead of a matrix with just one row (or the other way around). Can you please provide in all the combinations few screenshots with the signal dimensions displayed (you can find the right button in the menu)?

As a reference, here below you can find the logic that defines input port dimensions:

wb-toolbox/toolbox/library/src/QpOases.cpp

Lines 145 to 203 in 0688771

	// INPUTS
	// ======
	//
	// 1) Hessian Matrix (nV x nV)
	// 2) Gradient vector (1 x nV)
	// 3) Optional: Constraints matrix (nC x nV)
	// 4) Optional: Constraints lower bounds (1 x nV)
	// 5) Optional: Constraints upper bounds (1 x nV)
	// 6) Optional: Variables lower bounds (1 x nC)
	// 7) Optional: Variables upper bounds (1 x nC)
	//
	// OUTPUT
	// ======
	//
	// 1) Primal solution (1 x nV)
	// 2) Status of the qp solver (1x1)
	// 3) Optional: Value of the object function (1x1)
	InputPortsInfo inputPortsInfo;
	OutputPortsInfo outputPortsInfo;
	// Inputs
	inputPortsInfo.push_back({InputIndex::Hessian,
	Port::Dimensions{Port::DynamicSize, Port::DynamicSize},
	Port::DataType::DOUBLE});
	inputPortsInfo.push_back(
	{InputIndex::Gradient, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE});
	outputPortsInfo.push_back(
	{OutputIndex::PrimalSolution, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE});
	outputPortsInfo.push_back({OutputIndex::Status, Port::Dimensions{1}, Port::DataType::DOUBLE});
	// Optional inputs
	size_t numberOfInputs = InputIndex::Gradient;
	if (useLbA || useUbA) {
	InputIndex_constraints = ++numberOfInputs;
	inputPortsInfo.push_back({InputIndex_constraints,
	Port::Dimensions{Port::DynamicSize, Port::DynamicSize},
	Port::DataType::DOUBLE});
	}
	if (useLbA) {
	InputIndex_lbA = ++numberOfInputs;
	inputPortsInfo.push_back(
	{InputIndex_lbA, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE});
	}
	if (useUbA) {
	InputIndex_ubA = ++numberOfInputs;
	inputPortsInfo.push_back(
	{InputIndex_ubA, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE});
	}
	if (useLb) {
	InputIndex_lb = ++numberOfInputs;
	inputPortsInfo.push_back(
	{InputIndex_lb, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE});
	}
	if (useUb) {
	InputIndex_ub = ++numberOfInputs;
	inputPortsInfo.push_back(
	{InputIndex_ub, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE});
	}

[gabrielenava]

Sure, here are the screenshots with the signal dimensions:

QP with inputs bounds only

QP with inputs bounds and inequalities

I also drop again the error message, because I think the message got cut in the above comment while now I see the full error:

[diegoferigo]

Can you replace the all blocks from e.g. 2*ones(2,1) to 2 * [1; 1]? Try to get a matrix as output.

[gabrielenava]

I did it for every element:

But it didn't solve the issue. The error is the same.

[diegoferigo]

Mmh ok it was worth trying. Breaking down the error message, it seems that the stack trace is the following:

1. Unsupported number of port dimensions for port at index from SimulinkBlockInformationImpl.cpp#L289-L291
2. Input port at index 5does not contain a vector. Failed to get its size. from SimulinkBlockInformation.cpp#L49-L62
3. Sizes of bounds do not match with the number of variables. from QpOases.cpp#L266-L277

The origin of the error seems to be 1, somehow the call of ssGetInputPortNumDimensions(simstruct, idx) in the switch statement does not return 1 nor 2 (for, respectively, vector and matrix signals). Can you try to print its value in the two cases with only one QpOases block and with two of them?

[gabrielenava]

Ok I'll try. I also noted one thing I believe is important: the error message is generated when processing QP Test 2, that in my example is the QP block without inequality constraints, so it has a total of 4 input ports. However the error is generated when trying to process input port with index 5, which does not exist in that QP block.

[diegoferigo]

Good catch, this was not clear. Can you check under the mask if the S-Function is properly connected in the block that fails with only 4 inputs? I suspect that there's something strange going on there.

[gabrielenava]

I printed a message to check the output of ssGetInputPortNumDimensions(simstruct, idx) which is used in the switch-case logic.

Here is the result:

so it exits almost immediately after detecting a port with dimensions 0. If I comment out TEST QP3, the input ports are always either of dimension 1 or 2 (even if I don't know why the check is called so many times/ the simulation lasts 1 single step time):

this is the result of pressing: look under the mask on the QP block:

[nunoguedelha]

@diegoferigo @gabrielenava , I was happy to find this issue. I ran into another use case which is also failing, in this model.

The model is based on floating-base-balancing-torque-control, but the configuration of the QP blocks were the same: [H, g, A, ubA] inputs for both blocks. The issue disappeared after adding the input "lbA" to both blocks.

Considering a similar issue found by @Giulero , it looks like the QP block is likely to fail if we use any but the following configurations:

• unconstrained: H, g
• bounded constraints: H, g, A, lbA, ubA
• bounded constraints and variable: H, g, A, lbA, ubA, lb, ub

So the QP block logic checking the ports dimensions doesn't handle well leaving holes in the port list, i.e. "skipping" ports, if a QP block with all the inputs H, g, A, lbA, ubA, lb, ub also exists.

As per this hypothesis, the following will always work:

• H, g, A, lbA
• H, g, A, lbA, ubA, lb

... and the following will fail, unless no fully activated QP block exists in the same model:

• H, g, A, (missing lbA), ubA
• H, g, (missing A, lbA, ubA), lb, ub

[nunoguedelha]

@diegoferigo , @traversaro, I hope you don't mind, I added this to the board of element software engineering (next sprint tag), just for tracking it closer.

[diegoferigo]

@diegoferigo , @traversaro, I hope you don't mind, I added this to the board of element software engineering (next sprint tag), just for tracking it closer.

As long as you remember that this repo is public and all links to private repos cannot be reached by external users (like the element you linked above), is fine for me :)

So the QP block logic checking the ports dimensions doesn't handle well leaving holes in the port list, i.e. "skipping" ports, if a QP block with all the inputs H, g, A, lbA, ubA, lb, ub also exists.

I remember having quite a lot of problems with the signal dimensions, that required dirty workarounds like adding in the simulink side extra blocks that were forcing some of the output signals sizes match some of the inputs sizes. I don't have on the fly a working Matlab setup, but if you look under the mask of the block you will surely understand what I mean (if you can, please also attach a screenshot in this issue).

That workaround was working fine in the Matlab version I had at that time, I won't be surprised if it does not work reliably on new versions.