Make PostureTask great again

[haudren]

Sorry for the clickbait title 😼

Currently tasks::qp::PostureTask is a bit of an outlier: it is a direct tasks::qp::Task while most other are tasks::qp::HighLevelTask. The other offenders are either:

• Multi-robot related tasks (They can also probably be modified)
• Force related tasks (TorqueTask, ContactTask)

The only thing standing in the way of making it a HighLevelTask is to define it... Indeed, it already behaves as a SetPointTask with the assumption that the jacobian is constant, equal to something that looks like the identity:

• Q is set to J but it is equal to J^TJ in that case
• normalAcc is omitted because \dot{J} = 0

Making it a HighLevelTask would allow access, among other things to JointsSelector, and allow to set reference velocity/acceleration.

[jorisv]

Hello,

Can this be related to #21 ?
LSSOL support to directly solve the least square problem ||A_i x + b_i||^2_{W_i} instead of the quadratic one x^TQx + c^Tx.
This can be more faster and more reliable (instead of inverting Q the solver can decompose the A_i matrix).
As I remember a HighLeverTask is a kind of least square one that it turned into a quadratic one with some control stuff.

If we directly give least square instead of quadratic cost function to the solver then we will need to do what you said (Porting multi robot, Posture and force tasks to HighLevelTask).
So we definitely need to drain this swamp before accomplish #21.

[jovenagravante]

Hi,
I think the issue pointed out in #21 is the existence of a "default" posture task which should probably be handled in another way as suggested there.

As for which abstraction the posture task belongs - I would prefer to allow it to inherit set-point or trajectory task behaviours (this is what is proposed if I understand correctly). As for using JointSelector with the posture task, I think this was somehow possible with the old python bindings through some trick but probably refactoring is the way to go.

I think tasks is overdue for an overall refactor and cleanup - so much alternative code.

[jorisv]

Ok, I read #21 too fast.

JointSelector has never work with PostureTask since it's kind of impossible to cancel a joint on an Hessian.

I'm still a huge fan of this pull request since turning all constraint into a least square one could increase speed and reliability on some solvers.

[jovenagravante]

My mistake, I found the piece of code I was referring to - it's not using JointSelector. The workaround was to set individual JointStiffness differently so the effect is almost like JointSelector.

[gergondet]

Avoid raw Tasks

[wuv-ogre]

I was playing around with PostureTask to see what it did and found in

PostureTask::update
else if(mb.joint(i).dof() == 4)
for spherical joint should dof is 3, param is 4

[haudren]

You are right, this is a real bug. I don't think anyone ever truly used Tasks for spherical joints. Might warrant its own issue as this one is more concerned with refactoring / cleanup.