ERROR: Premature homotopy termination because QP is infeasible ->ERROR: Initial QP could not be solved due to infeasibility!

[lpenco]

I edited the simulink scheme of the controller torqueBalancing in https://github.com/robotology-playground/WBI-Toolbox-controllers. I'm using the SM COORDINATOR. I'm sending through 2 yarp ports the jointAngles and the CoM coordinates wrt to l_sole, respectively. The motion is a simple motion with the arms with the 2 feet on the ground, that works if retargeted on the old C++ torqueBalancing even if a bit unstable. In this case I edited the simulink in this way:

These are the signals at the beginning of the simulation when the robot doesn't move and waits for the commands from the read ports:

When sending the references these are the signals from the probes:

the robots first moves toward the right foot and then falls down giving then the error. I'm using the following gains:

Introducing an extreme smoothing the situation is the following: The robot does not fall but does the exact same movemnet as before toward the right foot but if I waited for way longer I'm sure it would have fallen, generating the error. Indeed the simulation on gazebo runs extremely slow even when gazebo is running on another computer, the real time factor goes even to 0 sometimes.

[gabrielenava]

The motion is a simple motion with the arms with the 2 feet on the ground

I imagine the (quite big) oscillations in the CoM references are a transient phase before reaching the steady-state value (the constant references after the oscillations), am I right? Because the amplitude of the CoM oscillations is 10-20cm, and there's no way the robot can handle such a big disturbance. The smoothing acts as a filter, hence it is good to have a very long smoothing time. It is necessary to avoid the robot to follow the -20cm reference on the y-CoM direction, otherwise I think it will happens exactly what you pointed out, i.e. the robot goes on the right foot and then fall down (it is trying to move the CoM beyond the right foot). Even better, is it possible to send directly the constant references after the 4s of simulation, without the transient?

To check if the constant references are fine (i.e. the final home position still has the CoM inside the support polygon), an easy way might be to modify these lines of the icub sdf to put the robot in the desired final position. The line I underlined is for the right arm references. If you modify the joint positions (same order of the yarpmotorgui) by putting your joint references (in radians), and then you put the robot in Gazebo, the robot will be in your reference position. You can check if your final position is fine (i.e., the robot doesn't fall) or if it is not a stable position instead.

Indeed the simulation on gazebo runs extremely slow even when gazebo is running on another computer, the real time factor goes even to 0 sometimes.

If the simulation is really slow, have a look at this comment. Regarding the real time factor, it must go to zero when Simulink is running, in fact this is a result of the synchronization between Simulink and Gazebo. If it is not zero (inside Gazebo), they are not correctly synchronized. Remember to run Gazebo as follows (run the yarpserver before): gazebo -slibgazebo_yarp_clock.so. If you have doubts, have a look at the documentation on how to run torque balancing simuations with iCub.

[lpenco]

The big oscillation is the reference I'm sending. Then the value stabilizes to the current position of the com since the reference is to keep the last position. Even though it is a big oscillation, since it corresponds to the com of the reference joint postures it could be handled by the robot. Indeed, the C++ controller works smoothly with this references, even without smoothing any trajectory. It accomplishes the task but it has the problem that the robot tends to bend the torso a little bit on the left or it does something else strange according to the kind of retargeted motion due to the zero-dynamics instability. I'm sending the references every 10ms and the controller period is 10ms so if gazebo, simulink and the references sending port are all sinchronyzed, is not even a problem of timing.

If it works on the c++ controller it has to work also in this one (if the controller has just be changed according to https://arxiv.org/pdf/1603.04178.pdf). Could it be a problem of reference frames at this point? or maybe the feet are not constrained to the stay attached to the ground like in the C++ version? In this last case either I could try to add this constraints and/or to smooth the initial trajectory.

[lpenco]

Yes I found where the problem was, I was using the zCoM that I send, as a threshold signal for a switch port and as a reference at the same time. That was causing troubles. I now use another signal and it the simulation runs smoothly. The robot falls down but just because the initial position doesn't match. I try now to smooth it further. Anyways I would like to add the constraint of the feet attached to the ground to give the robot further stability (like in the C++ version). Is there an easy way to do it, maybe just some conf parameter to set?

[gabrielenava]

Anyways I would like to add the constraint of the feet attached to the ground to give the robot further stability (like in the C++ version). Is there an easy way to do it, maybe just some conf parameter to set?

yes there is, and it consists in installing icub-gazebo-wholebody repository. If you installed the codyco-superbuild with the option CODYCO_USES_GAZEBO = ON, it is just a matter of adding the path to the model. You can follow this README for this. Then, inside Gazebo use the model icub (feet fixed) instead of icub (no hands).