variables vars to qp

[wuv-ogre]

The problem I'm interested in you've probably seen before

`centroidal momentum A, wrenches G

momentum constraints

G ρ = A vdot + Adot v
ρ >= 0

variables

x = [ qdot		ρ ]

equality constraint

Aeq * x				+ 	Beq 	= 0


G ρ = A vdot + Adot v
     
A vdot - G ρ 		+	Adot v  = 0
     
[ A		-G ] x	 	+ 	Adot v 	= 0

Aeq = [ A       -G ]
Beq = Adot v

inequality constraint

Aineq * x			+ 	Bineq	>= 0

    
ρ >= 0
     
[ 0		I ] x		+ 	0		>=0
     
Aineq = [ 0		I ]
Bineq = 0

LS ρ damping objective

ρᵀ Rho ρ`

So I wanted to add ρ which can change on a per frame basis to the qp vars. I called something like this VariableVars.

It does require updateVariableVars() to be called before solve()

solver.updateVariableVars(mbs); solver.solve(mbs, mbcs);

I haven't finished my momentum constraints yet, and so only tested for a single multibody plus a rand nr of LS damping VariableVars.

But I wondered what you thought of this in general?

Another question I have is why constraints don't have the same sort of begin() that tasks do i.e. why we write whole lines to the constraint matrices rather than blocks like with Q?

Also included a small fix for PostureTask, where dof and params got mixed up.