Improve solver

[rhaschke]

See individual commit messages again. I fixed some issues in the configuration of the QPs, but I still have weird behaviour: Both shown tasks exhibit substantial deviations from the already achieved solution. The effect is stronger the larger the updates dq are.

pos + ori on same level	pos > ori

[rhaschke]

Thinking more about it, the new handling of higher priority tasks only works for equality constraints. Inequality tasks should extend their bounds one-sided.

[felixkol]

First of all, thanks for your effort in renaming the variables and documentation!

For the two videos, I'm not quite sure what you mean by deviation from the solution already reached. But if I understand correctly it is as follows, using the example Pos + Ori on same level:

Moving the X axis: Since the minimum possible joint velocities are to be calculated, it is more advantageous not to move along the x-axis on a direct path, but to deviate from it. At the same time the orientation constraint takes effect, which can be solved exactly (without slack) therefore the target remains upright during the whole trajectory.

Rotation: Since Ori + Pos are on the same level, to solve the new pose it is possible to deviate from the already reached position by means of slack in order to move as fast as possible Ori- and! Pos-errors as quickly as possible. The opposite can be seen in the second video, where it is not allowed to deviate from the position to minimize the orientation.

[felixkol]

Thinking more about it, the new handling of higher priority tasks only works for equality constraints. Inequality tasks should extend their bounds one-sided.

'nullspace constraint is J' dq = fixed instead of lb - slack ≤ J' dq ≤ ub + slack' You mean this part, right? I didn't understand why we should clamp J' to the fixed solution, without the possibility to get better with subsequent tasks, keeping the inequality constraints (for inequality tasks).

[rhaschke]

For the two videos, I'm not quite sure what you mean

Compare the behavior with the standard inverse-Jacobian solver from the lecture. I don't remember such large straight-line deviations from this simple controller.

I didn't understand why we should clamp J' to the fixed solution, without the possibility to get better with subsequent tasks, keeping the inequality constraints

The found solution already is the optimal one. There is no chance of finding a better solution with subsequent tasks. Using an equality constraint allows for more efficient handling of the constraint.