WLQP C++ implementation and basic testing

avikde commented 4 years ago

Related to #171

avikde commented 4 years ago

WIth poptsEmp.npy

122.895 9.41853 12.146 2.03051

with popts

134.838 -0.0449229 -0.0796914 0.0236761

Should just test against the python implementation.

avikde commented 4 years ago

Comparing against python:

C++ 135 -0.0100647 -0.00349838 0.0100001, py 145.000193 0.00808 -0.000001 -0.001209

Python P, q, L, U, A

[[  0.001191  -0.000187   0.000002  -0.      ]
 [ -0.000187   1.276965   0.000028  -0.000008]
 [  0.000002   0.000028 202.582512   0.031697]
 [ -0.        -0.000008   0.031697   0.003323]] 

[-0.650178 -0.009384  0.000225  0.000005] 

[-5.   -0.01 -0.01 -0.01] 

[5.   0.01 0.01 0.01] 

[[1. 0. 0. 0.]
 [0. 1. 0. 0.]
 [0. 0. 1. 0.]
 [0. 0. 0. 1.]]

[145.000193   0.00808   -0.000001  -0.001209]

C

    359.012       20188     218.739    -2.08936
      20188 1.54481e+07      167411    -4029.28
    218.739      167411     1815.37     -44.033
   -2.08936    -4029.28     -44.033     23.4951   

26981.9
2.8276e+06
   30640.5
  -531.982  

 -5
-0.01
-0.01
-0.01   

5
0.01
0.01
0.01

1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1

Python w0, dwdu0

[-0.007069 -0.        2.14161   0.000042 -0.025544  0.000001] 

[[-0.000064  0.044836 -0.       -0.      ]
 [ 0.       -0.       -0.673496 -0.012121]
 [ 0.034512 -0.000003 -0.000002 -0.      ]
 [ 0.        0.000006 44.95844   0.004573]
 [-0.000515  3.570651  0.000004 -0.000022]
 [ 0.        0.000001  0.16686   0.178162]]

Update: see below for matching C++

C w0, dwdu0

   0.267156
   -719.418
3.34755e-05
   0.520927
   -4036.43
   -99.1444    

0.003901    -0.415712     0.299234   -0.0497393
    -5.13636     -3930.41     -42.5938      1.02509
 4.89247e-07     0.145529 -2.72507e-05 -8.71732e-06
-1.07942e-05   0.00877395      3.21959   0.00246728
    -57.6604  -7.6888e-05    0.0119702    -0.876442
    -1.25545    -0.187114    -0.237575      14.9549

avikde commented 4 years ago

C after row major fix

-0.00706892
-1.4918e-07
    2.14161
4.21319e-05
 -0.0255441
1.20551e-06-

6.36585e-05    0.0448362 -6.28635e-08 -5.12584e-05
 1.87453e-08 -4.91448e-09     0.994414    0.0760905
   0.0345117 -2.87201e-06  2.21965e-05  -0.00434314
 3.84269e-07   5.9398e-06     -28.0689      1.51934
-0.000514696      3.57065 -0.000249432  -0.00861035
 5.40621e-08  7.19318e-07      2.91534     -3.91778

Still some difference in dw/du.
Print out the matrix A2 and test the selfadjointview thing more

Update after comment below funapprox issue

C++

-0.00706892
-1.4918e-07
    2.14161
4.21319e-05
 -0.0255441
1.20551e-06

-6.36585e-05    0.0448362 -1.86813e-08 -1.29588e-08
 1.87453e-08 -4.91448e-09    -0.673496   -0.0121205
   0.0345117 -2.87201e-06 -1.90502e-06 -2.47343e-07
 3.84269e-07   5.9398e-06      44.9584   0.00457346
-0.000514696      3.57065   3.6243e-06 -2.22526e-05
 5.40621e-08  7.19318e-07      0.16686     0.178162

avikde commented 4 years ago

Python A2

[[-0.000001  0.001038 -0.       -0.      ]
 [ 0.        0.000309 -0.       -0.      ]
 [ 0.        0.       -0.012528 -0.238672]
 [ 0.        0.        0.       -0.011289]]

[[-0.000001  0.001038 -0.       -0.      ]
 [ 0.001038  0.000309 -0.       -0.      ]
 [-0.       -0.       -0.012528 -0.238672]
 [-0.       -0.       -0.238672 -0.011289]]

a2 and A2

[-0.000001  0.001038 -0.       -0.        0.000309 -0.       -0.       -0.012528 -0.238672 -0.011289] 

[[-0.000001  0.001038 -0.       -0.      ]
 [ 0.        0.000309 -0.       -0.      ]
 [ 0.        0.       -0.012528 -0.238672]
 [ 0.        0.        0.       -0.011289]]

C++ A2, and then after selfadjointView

-8.56113e-07   0.00103784 -2.46828e-09 -3.68507e-07
           0  -2.1527e-09  0.000308951   -0.0125276
           0            0 -5.49897e-08    -0.238672
           0            0            0   -0.0112893

-8.56113e-07   0.00103784 -2.46828e-09 -3.68507e-07
  0.00103784  -2.1527e-09  0.000308951   -0.0125276
-2.46828e-09  0.000308951 -5.49897e-08    -0.238672
-3.68507e-07   -0.0125276    -0.238672   -0.0112893

Physical popts -8.561129105121841e-07, 0.0010378391603621869, -2.1526976705866592e-09, -2.4682847177637722e-09, 0.00030895094854226844, -5.498973958767363e-08, -3.6850719187462874e-07, -0.012527592056397634, -0.2386723461724283, -0.011289258444268205

Found the issue:

Filling the upper triangular is row major in python

C++ A2 now matches python

-8.56113e-07   0.00103784  -2.1527e-09 -2.46828e-09
           0  0.000308951 -5.49897e-08 -3.68507e-07
           0            0   -0.0125276    -0.238672
           0            0            0   -0.0112893

-8.56113e-07   0.00103784  -2.1527e-09 -2.46828e-09
  0.00103784  0.000308951 -5.49897e-08 -3.68507e-07
 -2.1527e-09 -5.49897e-08   -0.0125276    -0.238672
-2.46828e-09 -3.68507e-07    -0.238672   -0.0112893

avikde commented 4 years ago

Confirmed that python and C match.

avikde / robobee3d

WLQP C++ implementation and basic testing #173