Pause Walking unexpectedly triggered during motion

[Ahmadreza-Shahrokhshahi]

Hi Stephane,

I am simulating the walking on a curb (single step) and the motion is automatically paused/stopped sometimes. It was first happening when I was simulating with greater heights, but in the recent simulations it also happens in the standing phase sometimes. Here are some explanation points:

1. For curb-up it happens exactly when the first SSP is finished (the moment that the swing foot lands on the curb)
2. It is not repeatable; happens sometimes and completes the motion the other times (keeping exactly the same conditions)
3. It also happens not only in the simulation, but also in the experimental tests
4. There is no error/warning/etc message when this happens
5. Tracking the joint commands was not that perfect, so I increased all P-gains and some of D-gains. It made it better, but still happens.

It would be great to have your comments about the possible reasons for this issue.
Thanks

P.S. I have attached photos of the moment this pause happens in Standing and Curb-up motions, and also the terminal where I run Choreonoid.

[stephane-caron]

From your last screenshot it seems the controller is stuck in SingleSupport state?

I have never run into that one. I'm not sure how it would happen if you are using SingleSupport.cpp from this repository, since the state's logic is very simple regarding transitions:

• runState() decreases remTime_ no matter what,
• no other function increases it (it is only reset once at start()), and
• the state will transition to double support when this variable becomes negative.

Anyhow, this is easy to check: you can print out remTime_ in your terminal to check that it is initialized properly and eventually decreases below zero. I wouldn't be surprised one of these two checks fails when you get an undesirable pause.

On a side note: judging from your other two screenshots, you are using jrl-umi3218's maintained version of the controller, which is a good idea 😉 It is better to ask your question on the corresponding issue tracker then, so that package maintainers can also chime in (e.g. if this would be due to some change in another part of the framework).

[Ahmadreza-Shahrokhshahi]

@arntanguy
Hi Arnaud,
Regarding the issue opened here, I wanted to know your opinion too. After Stephane suggested to print the remTime_ in SingleSupport.cpp to see if it goes below zero to start the next state, I saw that the motion stops: sometimes when it doesn't go below zero and sometimes when it goes below zero and the double support starts (screen-shots attached).

It should be mentioned that after this happens, I have to force quit the terminal for closing Choreonoid. Another point is that I think the transition between states does not happen only if remTime_ reaches below zero, the ContactState would also change based on the increase of the vertical forces more than a threshold.

Please let me know if you have ideas to resolve this issue.
Thanks

[arntanguy]

Hi,

We figured out the root cause of this issue in our internal bug tracker a while ago, sorry I forgot to report it here.
It seems that sometimes, when Quadprog cannot find a solution to the wrench distribution problem, the solver gets stuck in an infinite loop, instead of stopping as it should. Here is a stacktrace when the freeze happens (using mc_rtc's LIPMStabilizer, which is for all intent and purposes the same as the Stabilizer here).
This seem to happen most often when the problem is quite complex, I've personally only seen failures when stepping sideways onto a rather large block and rarely while walking with a complex online footstep plan.

Thread 1 "MCUDPControl" received signal SIGINT, Interrupt.
qpgen2 (dmat=..., dvec=..., fddmat=12, n=12, sol=..., 
    crval=-705717843.97544503, amat=..., bvec=..., fdamat=12, q=34, 
    meq=0, iact=..., nact=4, iter=..., work=..., ierr=0)
    at /home/julien/MC_RTC/eigen-quadprog/src/QuadProg/f/solve.QP.f:270
270	         work(iwrv+i) = sum
(gdb) bt
#0  qpgen2 (dmat=..., dvec=..., fddmat=12, n=12, sol=..., 
    crval=-705717843.97544503, amat=..., bvec=..., fdamat=12, q=34, 
    meq=0, iact=..., nact=4, iter=..., work=..., ierr=0)
    at /home/julien/MC_RTC/eigen-quadprog/src/QuadProg/f/solve.QP.f:270
#1  0x00007ffff316bfed in Eigen::QuadProgDense::solve (
    this=this@entry=0x555555bab928, q=34, C=..., Aeq=..., Beq=..., 
    Aineq=..., Bineq=..., isDecomp=false)
    at /home/julien/MC_RTC/eigen-quadprog/src/QuadProg.cpp:147
#2  0x00007ffff5a5f1d5 in mc_tasks::lipm_stabilizer::StabilizerTask::distributeWrench (this=this@entry=0x555555bab100, desiredWrench=...)
    at /home/julien/MC_RTC/mc_rtc/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp:761
#3  0x00007ffff5a6217b in mc_tasks::lipm_stabilizer::StabilizerTask::run (this=this@entry=0x555555bab100)
    at /home/julien/MC_RTC/mc_rtc/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp:595
#4  0x00007ffff5a62317 in mc_tasks::lipm_stabilizer::StabilizerTask::update (this=0x555555bab100, solver=...)
    at /home/julien/MC_RTC/mc_rtc/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp:238
#5  0x00007ffff56dbb51 in mc_solver::QPSolver::runOpenLoop (
    this=0x55555588e680)

We haven't investigated further, and for the time being we internally reverted to using the LSSOL solver which doesn't suffer from this issue and always manages to find a solution. Unfortunately we can't release this publicly, so you're stuck with trying to figure out why quadprog gets stuck looking for a solution. It may also be that the problem is ill-formed at that moment, and in that case we'd need to figure out if there is anything wrong with the wrenchDistribution implementation/formulation.

According to the stack trace, above, it seems to get stuck somewhere in the fortran code of the solver. However it got interrupted here, so the reported error line is not necessarely here it fails (but it's likely to be in that particular loop).

c
c and r = R^{-1} d_1, check also if r has positive elements (among the 
c entries corresponding to inequalities constraints).
c
      t1inf = .TRUE.
      do 95 i=nact,1,-1
         sum = work(i)
         l  = iwrm+(i*(i+3))/2
         l1 = l-i
         do 96 j=i+1,nact
            sum = sum - work(l)*work(iwrv+j)
            l   = l+j
 96      continue
         sum = sum / work(l1)
         work(iwrv+i) = sum # 
         if (iact(i) .LE. meq) goto 95
         if (sum .LE. 0.d0) goto 95
 7       t1inf = .FALSE.
         it1 = i
 95   continue

I don't have time right now to try and look further into it, and am definitely not familiar with the internals of quadprog. Maybe @aescande has some further insights?

[Ahmadreza-Shahrokhshahi]

Thank you all,
Based on the responses you made, I think we would have three main options, which I am mentioning in order to have your ideas:

@aescande, @arntanguy

1. Keep the current solver Quadprog with the following possible ways:
   • Modify the inputs to the QP such that the ill-condition does not happen
   • Detect the ill-condition to ignore, or replace the previous step values for the wrench distribution
   • Make the solver to always find a solution, no matter what the condition is
     (I am not sure if any of the items could be possible in practice)

@aescande, @arntanguy @stephane-caron

2. Switch to another solver; if you have tried LSSOL or any other solver that could manage this condition better. If it's LSSOL and it's not publicly available, what's your suggestion? How can we have access to that? (purchasing, required modifications in the codes, etc.)

@stephane-caron

3. Completely ignore a QP solver for the wrench distribution problem, and use algebraic system of equations to simply find a solution for the problem. Please advise if you have any experience on this, or have tried it before.

[arntanguy]

Hi,

That's a good summary of your options.

• Regarding 1
  Point 1 and 2 seem hard to ensure in practice. Point 3 seems like the one we should try and figure out: the solver is best placed to know whether a problem does indeed have a solution or not. If we wish to try and debug why Quadprog end-up getting stuck in it's internal computations, it'd be helpful to be able to reproduce the problem on a numerical example. To that end, it'd be helpful to dump the problem matrices from the wrenchDistribution problem, and create a simple C++ test that just attempts to compute the problem from these inputs. It should get stuck just the same there, and would allow to peer more deeply into the internals of Quadprog to try and understand what it does (cc @aescande). We'd certainly be grateful if you can dedicate some energy to setting up such a reproducible test.

• Regarding 2
  We have the implementation for LSSOL, and we could share that if you have access to the solver. As far as I know, they have academic and commercial licences, but I'm not aware of the details.
  There is no strong tie to using Quadprog, so other solvers could be tried. We currently have Eigen binding for eigen-quadprog, eigen-qld, eigen-osqp and eigen-gurobi, although I'm not sure which would be best suited for that particular problem. I'll let @aescande chim in his toughts ;)

[Ahmadreza-Shahrokhshahi]

Thank you all,
Based on the responses you made, I think we would have three main options, which I am mentioning in order to have your ideas:

@aescande, @arntanguy

1. Keep the current solver Quadprog with the following possible ways:
   
   * Modify the inputs to the QP such that the ill-condition does not happen
   * Detect the ill-condition to ignore, or replace the previous step values for the wrench distribution
   * Make the solver to always find a solution, no matter what the condition is
     (I am not sure if any of the items could be possible in practice)

@aescande, @arntanguy @stephane-caron

1. Switch to another solver; if you have tried LSSOL or any other solver that could manage this condition better. If it's LSSOL and it's not publicly available, what's your suggestion? How can we have access to that? (purchasing, required modifications in the codes, etc.)

@stephane-caron

1. Completely ignore a QP solver for the wrench distribution problem, and use algebraic system of equations to simply find a solution for the problem. Please advise if you have any experience on this, or have tried it before.

That would be great if I could have Stephane's and Adrien's opinions as well @stephane-caron @aescande
Thanks

[aescande]

Hi there,

doing anything at Quadprog's level (option 1) will likely be tricky, but will always start by having a repro code for the problem. I don't think the solver can be stuck in the exact loops that Arnaud pinpointed, but it's more likely in the outer loop that contains this code. The easiest patch could be to put a maximum iteration number on this outer loop. I don't have time to delve into the fortran code right now, but I could review a patch.
But again repro code first.

About the different QPs (option 2): Quadprog was chosen because it was faster than LSSOL for this problem, but LSSOL works fine. There are plenty of other QP solvers out there, some for which we have Eigen wrapping, but I cannot comment on their robustness with respect to your problem. As Arnaud said, LSSOL is available for licensing. The cost of an academic licence is iirc 200-300$.
I have a prototype c++ implementation of the same Goldfarb-Idnani paper that is behind Quadprog and should make it available this summer, but it's not ready for release yet, will not be battle-tested, and I won't work on it in the coming weeks.

About algebraic resolution: do you have anything specific in mind ? As far as I know, algebraic resolution with inequalities quickly becomes impracticable (we're looking at exponential or double exponential complexity in the problem dimension here).

[stephane-caron]

Waow! Good to know about these corner cases with eigen-quadprog 😀

Switching QP solver

@Ahmadreza-Shahrokhshahi I guess the closest option for you then would be to switch the Stabilizer class from eigen-quadprog to eigen-qld. The API of the solve function is the same.

Removing the QP solver

Completely ignore a QP solver for the wrench distribution problem, and use algebraic system of equations to simply find a solution for the problem. Please advise if you have any experience on this, or have tried it before.

By "algebraic system of equations", I'm interpreting you go for some generalized inverse (there are 2 contact wrenches summing up to 1 net wrench, so the problem is underdetermined), which is equivalent to an unconstrained least-squares optimization, which is also equivalent to removing contact wrench inequalities from the wrench distribution QP (WDQP).

I haven't tried that. For you it sounds like more work, since you would need to change not only the call to the QP solver in the code but also build another set of matrices. Assuming this is done, I would expect the resulting closed-loop system to behave:

• When no contact wrench inequality is active (e.g. CoPs well inside footholds): pretty much the same as the current WDQP.
• When some contact wrench inequalities become active (e.g. CoPs hit border of foothold polygons): way worse than WDQP, as it's going to command unfeasible wrenches to foot admittance tasks.

Bonus version: you can also solve unconstrained least-squares then clip wrench inequalities afterwards. This is still worse than WDQP, since numerically it will return a sub-optimum (whereas the QP solution is the constrained optimum), but it might work reasonably well.

Hoping this helps!

[Ahmadreza-Shahrokhshahi]

Thank you all for the updates and suggestions.
I'm closing this issue for now.
@stephane-caron @arntanguy @aescande