Analytical version of wrench gradient controller

[avikde]

Purpose is to have a solution that does not need osqp.

[avikde]

Analytical solution on the right - seems wrong.

[135.       0.0113   0.0001  -0.0101] [140.     0.01   0.     0.  ]
[130.       0.0211  -0.      -0.0201] [140.1176 840.4624   0.0347  -0.0199]
[125.       0.0311  -0.0002  -0.03  ] [10258.288   1689.5479    -0.0177  3382.115 ]
[120.       0.0411  -0.0003  -0.0393] [  24707.46    141621.39   4489526.        1691.2117]
[115.       0.0511  -0.0004  -0.0312] [5.3090e+11 7.0781e+04 2.2448e+06 2.2402e+15]
[110.       0.0611  -0.0005  -0.0324] [nan nan nan nan]

[avikde]

Printing out P,q from the two different methods:

wlqp:   0.00176758 -2.65288e-06  -0.00156757   0.00174774
-2.65288e-06      2.32714  0.000151151 -6.56168e-05
 -0.00156757  0.000151151      174.223    -0.890306
  0.00174774 -6.56168e-05    -0.890306      2.03738  0.101551
  -1.61641
-0.0378339
  0.140551
0.001768,-0.000003,-0.001568,0.001748,
-0.000003,2.327142,0.000151,-0.000066,
-0.001568,0.000151,174.223145,-0.890306,
0.001748,-0.000066,-0.890306,2.037380,
-0.101551,1.616412,0.037834,-0.140551,
[135.       0.0113   0.0001  -0.0101] [140.     0.01   0.     0.  ]

Conclusion:

• It is correct