Commit c600acd3 authored by flforget's avatar flforget
Browse files

[Major]QPBox now works for several commands system

previously QPBox only worked for single command system
parent 6123926a
......@@ -150,19 +150,17 @@ void ILQRSolver::backwardLoop()
lb = lowerCommandBounds - u;
ub = upperCommandBounds - u;
qp->init(H.data(),g.data(),lb.data(),ub.data(),nWSR);
//cout << i<<'\t' <<H <<'\t'<< g<<'\t' << lb<<'\t' << ub <<endl;
qp->getPrimalSolution(xOpt);
k = Map<commandVec_t>(xOpt);
//cout << k << endl<<'-'<<endl;
if((k == lowerCommandBounds) | (k == upperCommandBounds))
{
K.setZero();
cout << iter << " : "<< i << " -> " <<"bounded" << endl;
}
else
K = -QuuInv*Qux;
for(unsigned int i_cmd=0;i_cmd<commandNb;i_cmd++)
{
K = -QuuInv*Qux; // to be modified
if((k[i_cmd] == lowerCommandBounds[i_cmd]) | (k[i_cmd] == upperCommandBounds[i_cmd]))
{
K.row(i_cmd).setZero();
}
}
cout << K << endl<<endl;
}
else
{
......
......@@ -32,7 +32,7 @@ int main()
unsigned int T = 50;
double dt=1e-4;
unsigned int iterMax = 100;
unsigned int iterMax = 50;
double stopCrit = 1e-5;
stateVec_t* xList;
commandVec_t* uList;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment