arm.txt

Link [base_link] does not have an origin. Assuming this is the fixed world base frame. Else there is an error with your URDF file.
Link [base_link] does not have inertial properties. Assuming this is the fixed world base frame. Else there is an error with your URDF file.
------------------------------------------
Assumed Input Joint Configuration Ordering
------------------------------------------
joint1
joint2
----------------------------
Total of n = 2 joints
----------------------------
-----------------------------
-----------------------------
    Running SQP Example      
-----------------------------
-----------------------------
Solving Unconstrained Problem
-----------------------------
-----------------------------
Solving with method:  SQPSolverMethods.PCG_SS
cost 
 1.8099999999999998
cost 
 1.8099999999999998
cost 
 1.8099999999999998
cost 
 1.8099999999999998
cost 
 1.8099999999999998
cost 
 1.8099999999999998
cost 
 1.8099999999999998
cost 
 1.8099999999999998
cost 
 1.8099999999999998
cost 
 181.0
grad 
 [[-0.2 -0.1  0.   0.   0.   0. ]]
grad 
 [[-0.2 -0.1  0.   0.   0.   0. ]]
grad 
 [[-0.2 -0.1  0.   0.   0.   0. ]]
grad 
 [[-0.2 -0.1  0.   0.   0.   0. ]]
grad 
 [[-0.2 -0.1  0.   0.   0.   0. ]]
grad 
 [[-0.2 -0.1  0.   0.   0.   0. ]]
grad 
 [[-0.2 -0.1  0.   0.   0.   0. ]]
grad 
 [[-0.2 -0.1  0.   0.   0.   0. ]]
grad 
 [[-0.2 -0.1  0.   0.   0.   0. ]]
grad 
 [[-20. -10.   0.   0.]]
cost 
 1.8112877053521257
cost 
 2.2044683547702757
cost 
 2.144986696222268
cost 
 2.0188824849990725
cost 
 2.013872233865556
cost 
 2.0079866083084
cost 
 2.0360129454446785
cost 
 2.029061860023587
cost 
 2.1029872267096565
cost 
 176.05444492127222
grad 
 [[-0.2    -0.1001 -0.     -0.     -0.016   0.0016]]
grad 
 [[-0.2    -0.0999  1.7328  0.8664  0.0602  0.0146]]
grad 
 [[-0.1897 -0.291   1.5782  0.8062  0.0941  0.0228]]
grad 
 [[-0.1797 -0.3831  1.3774  0.6571  0.0689  0.0167]]
grad 
 [[-0.1733  0.055   1.2663  0.5899  0.0009  0.0001]]
grad 
 [[-0.1676 -0.0782  1.2672  0.6336 -0.0688 -0.0168]]
grad 
 [[-0.1591  0.1083  1.3406  0.6537 -0.0979 -0.0239]]
grad 
 [[-0.1499  0.2065  1.4574  0.7616 -0.0689 -0.0168]]
grad 
 [[-0.1421 -0.2006  1.576   0.8279  0.0024 -0.0046]]
grad 
 [[-13.3326  -7.0392  -0.0023  -0.0009]]
cost 
 1.8103219196613978
cost 
 1.9086170576216799
cost 
 1.8906774428597384
cost 
 1.8565122501338744
cost 
 1.8453976739068916
cost 
 1.8519117394639952
cost 
 1.8562622761226544
cost 
 1.8521289254202136
cost 
 1.8618151950784163
cost 
 178.28148531076286
grad 
 [[-0.2    -0.1    -0.     -0.     -0.008   0.0008]]
grad 
 [[-0.2    -0.1     0.8664  0.4332  0.0301  0.0073]]
grad 
 [[-0.1952 -0.2106  0.7983  0.4013  0.047   0.0114]]
grad 
 [[-0.1907 -0.2603  0.7058  0.3489  0.0345  0.0084]]
grad 
 [[-0.1874 -0.1689  0.6414  0.3153  0.0004  0.0001]]
grad 
 [[-0.1843 -0.0947  0.6336  0.3168 -0.0344 -0.0084]]
grad 
 [[-0.1806  0.0213  0.6781  0.337  -0.0489 -0.0119]]
grad 
 [[-0.1765  0.0733  0.7464  0.3773 -0.0344 -0.0084]]
grad 
 [[-0.1727 -0.0138  0.7974  0.4037  0.0012 -0.0023]]
grad 
 [[-16.8487  -8.6219  -0.0011  -0.0005]]
cost 
 1.8100804765770329
cost 
 1.8346542500734706
cost 
 1.8290291020122433
cost 
 1.8194439684534438
cost 
 1.815268526883446
cost 
 1.8166959014352952
cost 
 1.8169212838075341
cost 
 1.814988664938176
cost 
 1.8160366334088387
cost 
 179.58275592588114
grad 
 [[-0.2    -0.1    -0.     -0.     -0.004   0.0004]]
grad 
 [[-0.2    -0.1     0.4332  0.2166  0.015   0.0036]]
grad 
 [[-0.1977 -0.1572  0.4003  0.2004  0.0235  0.0057]]
grad 
 [[-0.1956 -0.1826  0.3551  0.177   0.0172  0.0042]]
grad 
 [[-0.1939 -0.153   0.3218  0.1602  0.0002  0.    ]]
grad 
 [[-0.1923 -0.0981  0.3168  0.1584 -0.0172 -0.0042]]
grad 
 [[-0.1906 -0.0372  0.3401  0.1698 -0.0245 -0.006 ]]
grad 
 [[-0.1887 -0.0108  0.3754  0.1882 -0.0172 -0.0042]]
grad 
 [[-0.1868 -0.0397  0.3999  0.2006  0.0006 -0.0011]]
grad 
 [[-18.4744  -9.3376  -0.0006  -0.0003]]
grad 
 [[-0.2    -0.1    -0.     -0.     -0.004   0.0004]]
grad 
 [[-0.2    -0.1     0.4332  0.2166  0.015   0.0036]]
grad 
 [[-0.1977 -0.1572  0.4003  0.2004  0.0235  0.0057]]
grad 
 [[-0.1956 -0.1826  0.3551  0.177   0.0172  0.0042]]
grad 
 [[-0.1939 -0.153   0.3218  0.1602  0.0002  0.    ]]
grad 
 [[-0.1923 -0.0981  0.3168  0.1584 -0.0172 -0.0042]]
grad 
 [[-0.1906 -0.0372  0.3401  0.1698 -0.0245 -0.006 ]]
grad 
 [[-0.1887 -0.0108  0.3754  0.1882 -0.0172 -0.0042]]
grad 
 [[-0.1868 -0.0397  0.3999  0.2006  0.0006 -0.0011]]
grad 
 [[-18.4744  -9.3376  -0.0006  -0.0003]]
cost 
 1.8117121673675118
cost 
 2.146162677381579
cost 
 2.0886912699631197
cost 
 2.029707523278982
cost 
 2.010747592996227
cost 
 2.0115266720389133
cost 
 2.021770400900861
cost 
 2.0219209438600743
cost 
 2.0402633557235927
cost 
 176.14118461190392
grad 
 [[-0.2    -0.1     0.      0.      0.0161  0.0092]]
grad 
 [[-0.2    -0.1     1.6029  0.8015  0.0532  0.0129]]
grad 
 [[-0.1911 -0.2678  1.4937  0.7504  0.0586  0.0142]]
grad 
 [[-0.1828 -0.2889  1.3956  0.6884  0.0361  0.0087]]
grad 
 [[-0.1758 -0.1458  1.338   0.6613 -0.0041 -0.0009]]
grad 
 [[-0.1686 -0.0747  1.3173  0.6585 -0.047  -0.0115]]
grad 
 [[-0.1604  0.094   1.3801  0.6861 -0.058  -0.0141]]
grad 
 [[-0.1519  0.1356  1.4394  0.729  -0.0395 -0.0096]]
grad 
 [[-0.1437 -0.0005  1.4852  0.7519  0.0002 -0.005 ]]
grad 
 [[-13.4928  -6.9865   0.0031   0.0015]]
cost 
 1.8102968820977017
cost 
 1.9457210417807798
cost 
 1.9208961905391675
cost 
 1.8908520074639703
cost 
 1.87952473577231
cost 
 1.8813381539531187
cost 
 1.8847283183051013
cost 
 1.882772152752788
cost 
 1.8892570908528832
cost 
 177.72831085089987
grad 
 [[-0.2    -0.1    -0.     -0.      0.006   0.0048]]
grad 
 [[-0.2    -0.1     1.0181  0.509   0.0341  0.0083]]
grad 
 [[-0.1945 -0.2086  0.9494  0.476   0.0411  0.0099]]
grad 
 [[-0.1893 -0.2373  0.8788  0.4363  0.0267  0.0064]]
grad 
 [[-0.185  -0.1618  0.8311  0.4127 -0.0019 -0.0004]]
grad 
 [[-0.1807 -0.0839  0.8171  0.4085 -0.0321 -0.0078]]
grad 
 [[-0.1759  0.025   0.8626  0.4299 -0.0412 -0.0101]]
grad 
 [[-0.1709  0.0617  0.9114  0.4588 -0.0284 -0.0069]]
grad 
 [[-0.166  -0.0078  0.9444  0.4754  0.0004 -0.0031]]
grad 
 [[-16.0759  -8.2113   0.0013   0.0006]]
cost 
 1.8100388193687245
cost 
 1.8790157904459124
cost 
 1.8654170133532568
cost 
 1.8467347433196386
cost 
 1.8391346693626045
cost 
 1.8408316853008815
cost 
 1.8421264348681903
cost 
 1.839950177106392
cost 
 1.843066939112242
cost 
 178.6233814571198
grad 
 [[-0.2    -0.1    -0.     -0.      0.001   0.0026]]
grad 
 [[-0.2    -0.1     0.7256  0.3628  0.0246  0.006 ]]
grad 
 [[-0.1961 -0.1822  0.6753  0.3383  0.0323  0.0078]]
grad 
 [[-0.1925 -0.2103  0.6176  0.3074  0.022   0.0053]]
grad 
 [[-0.1895 -0.1599  0.5767  0.2868 -0.0009 -0.0002]]
grad 
 [[-0.1866 -0.0905  0.567   0.2835 -0.0247 -0.006 ]]
grad 
 [[-0.1834 -0.0066  0.6018  0.3002 -0.0329 -0.008 ]]
grad 
 [[-0.1799  0.0253  0.6442  0.3235 -0.0228 -0.0055]]
grad 
 [[-0.1766 -0.0212  0.6725  0.3378  0.0005 -0.0021]]
grad 
 [[-17.3     -8.7873   0.0003   0.0002]]
grad 
 [[-0.2    -0.1    -0.     -0.      0.001   0.0026]]
grad 
 [[-0.2    -0.1     0.7256  0.3628  0.0246  0.006 ]]
grad 
 [[-0.1961 -0.1822  0.6753  0.3383  0.0323  0.0078]]
grad 
 [[-0.1925 -0.2103  0.6176  0.3074  0.022   0.0053]]
grad 
 [[-0.1895 -0.1599  0.5767  0.2868 -0.0009 -0.0002]]
grad 
 [[-0.1866 -0.0905  0.567   0.2835 -0.0247 -0.006 ]]
grad 
 [[-0.1834 -0.0066  0.6018  0.3002 -0.0329 -0.008 ]]
grad 
 [[-0.1799  0.0253  0.6442  0.3235 -0.0228 -0.0055]]
grad 
 [[-0.1766 -0.0212  0.6725  0.3378  0.0005 -0.0021]]
grad 
 [[-17.3     -8.7873   0.0003   0.0002]]
cost 
 1.8127759406884156
cost 
 2.134186664881243
cost 
 2.0774455533413265
cost 
 2.030716489615065
cost 
 2.011478882796233
cost 
 2.009601642872241
cost 
 2.0197593446833104
cost 
 2.018854593231261
cost 
 2.0311706043133024
cost 
 176.1751486838978
grad 
 [[-0.2    -0.1    -0.     -0.      0.0212  0.0103]]
grad 
 [[-0.2    -0.1     1.574   0.787   0.0524  0.0127]]
grad 
 [[-0.1914 -0.2572  1.4697  0.7371  0.0522  0.0126]]
grad 
 [[-0.1833 -0.269   1.3938  0.6899  0.0311  0.0075]]
grad 
 [[-0.1762 -0.1501  1.34    0.665  -0.005  -0.001 ]]
grad 
 [[-0.169  -0.0683  1.3151  0.6573 -0.0442 -0.0108]]
grad 
 [[-0.1609  0.0887  1.3829  0.6887 -0.0509 -0.0124]]
grad 
 [[-0.1524  0.1191  1.4284  0.7214 -0.0344 -0.0083]]
grad 
 [[-0.1442  0.0078  1.4618  0.7376  0.0001 -0.0051]]
grad 
 [[-13.5551  -6.9937   0.0033   0.0016]]
cost 
 1.8108249304153003
cost 
 1.9830897839931876
cost 
 1.9516092468409891
cost 
 1.91977804442016
cost 
 1.9066331127748102
cost 
 1.9070597811056156
cost 
 1.9117336026175662
cost 
 1.9098901172457274
cost 
 1.916681140212642
cost 
 177.325105238148
grad 
 [[-0.2    -0.1    -0.     -0.      0.0111  0.0065]]
grad 
 [[-0.2    -0.1     1.1498  0.5749  0.0385  0.0093]]
grad 
 [[-0.1938 -0.217   1.0736  0.5381  0.0422  0.0102]]
grad 
 [[-0.188  -0.2398  1.0071  0.5     0.0265  0.0064]]
grad 
 [[-0.1829 -0.1583  0.9587  0.4764 -0.0029 -0.0006]]
grad 
 [[-0.1779 -0.0781  0.9411  0.4705 -0.0344 -0.0084]]
grad 
 [[-0.1723  0.0383  0.9936  0.4953 -0.0419 -0.0102]]
grad 
 [[-0.1665  0.0711  1.0381  0.5226 -0.0286 -0.0069]]
grad 
 [[-0.1608 -0.0034  1.0679  0.5375  0.0003 -0.0036]]
grad 
 [[-15.476   -7.9183   0.0018   0.0009]]
grad 
 [[-0.2    -0.1    -0.     -0.      0.0111  0.0065]]
grad 
 [[-0.2    -0.1     1.1498  0.5749  0.0385  0.0093]]
grad 
 [[-0.1938 -0.217   1.0736  0.5381  0.0422  0.0102]]
grad 
 [[-0.188  -0.2398  1.0071  0.5     0.0265  0.0064]]
grad 
 [[-0.1829 -0.1583  0.9587  0.4764 -0.0029 -0.0006]]
grad 
 [[-0.1779 -0.0781  0.9411  0.4705 -0.0344 -0.0084]]
grad 
 [[-0.1723  0.0383  0.9936  0.4953 -0.0419 -0.0102]]
grad 
 [[-0.1665  0.0711  1.0381  0.5226 -0.0286 -0.0069]]
grad 
 [[-0.1608 -0.0034  1.0679  0.5375  0.0003 -0.0036]]
grad 
 [[-15.476   -7.9183   0.0018   0.0009]]
cost 
 1.8130538255285658
cost 
 2.1288112633145624
cost 
 2.0711362745469906
cost 
 2.030058936441848
cost 
 2.0093945444485573
cost 
 2.006583150087798
cost 
 2.018370284610101
cost 
 2.0162039044137767
cost 
 2.0263962966260416
cost 
 176.20083934384414
grad 
 [[-0.2    -0.1     0.      0.      0.0223  0.0106]]
grad 
 [[-0.2    -0.1     1.56    0.78    0.0525  0.0128]]
grad 
 [[-0.1915 -0.2538  1.4547  0.7293  0.0499  0.012 ]]
grad 
 [[-0.1836 -0.2618  1.3898  0.6886  0.0298  0.0073]]
grad 
 [[-0.1764 -0.1491  1.333   0.6623 -0.0054 -0.001 ]]
grad 
 [[-0.1692 -0.0653  1.3056  0.6525 -0.0442 -0.0108]]
grad 
 [[-0.1612  0.0894  1.3825  0.6889 -0.0482 -0.0117]]
grad 
 [[-0.1528  0.1134  1.42    0.7167 -0.0325 -0.0078]]
grad 
 [[-0.1446  0.009   1.4484  0.7301  0.0005 -0.0051]]
grad 
 [[-13.602   -7.0217   0.0024   0.0011]]
grad 
 [[-0.2    -0.1     0.      0.      0.0223  0.0106]]
grad 
 [[-0.2    -0.1     1.56    0.78    0.0525  0.0128]]
grad 
 [[-0.1915 -0.2538  1.4547  0.7293  0.0499  0.012 ]]
grad 
 [[-0.1836 -0.2618  1.3898  0.6886  0.0298  0.0073]]
grad 
 [[-0.1764 -0.1491  1.333   0.6623 -0.0054 -0.001 ]]
grad 
 [[-0.1692 -0.0653  1.3056  0.6525 -0.0442 -0.0108]]
grad 
 [[-0.1612  0.0894  1.3825  0.6889 -0.0482 -0.0117]]
grad 
 [[-0.1528  0.1134  1.42    0.7167 -0.0325 -0.0078]]
grad 
 [[-0.1446  0.009   1.4484  0.7301  0.0005 -0.0051]]
grad 
 [[-13.602   -7.0217   0.0024   0.0011]]
cost 
 1.8129275958130178
cost 
 2.1287399884758313
cost 
 2.069812839930891
cost 
 2.0295726158635157
cost 
 2.0082452017341903
cost 
 2.005376513704454
cost 
 2.0183247552065775
cost 
 2.015581258379403
cost 
 2.0256041743797946
cost 
 176.20666554074992
grad 
 [[-0.2    -0.1001 -0.     -0.      0.0218  0.0104]]
grad 
 [[-0.2    -0.0999  1.5591  0.7796  0.053   0.0129]]
grad 
 [[-0.1915 -0.2547  1.4513  0.7275  0.0499  0.012 ]]
grad 
 [[-0.1836 -0.2623  1.3883  0.6878  0.0301  0.0074]]
grad 
 [[-0.1765 -0.1483  1.3293  0.6604 -0.0055 -0.001 ]]
grad 
 [[-0.1693 -0.0652  1.3007  0.65   -0.0449 -0.011 ]]
grad 
 [[-0.1613  0.0913  1.3831  0.6892 -0.048  -0.0116]]
grad 
 [[-0.1529  0.1135  1.4185  0.716  -0.0323 -0.0077]]
grad 
 [[-0.1447  0.0088  1.446   0.729   0.001  -0.005 ]]
grad 
 [[-13.6127  -7.0358   0.0011   0.0005]]
cost 
 1.8129275958130178
cost 
 2.1287399884758313
cost 
 2.069812839930891
cost 
 2.0295726158635157
cost 
 2.0082452017341903
cost 
 2.005376513704454
cost 
 2.0183247552065775
cost 
 2.015581258379403
cost 
 2.0256041743797946
cost 
 176.20666554074992
Cost [ 194.3208504842376 ]
Final State Error vs. Goal
Position in x
[2.     2.     1.9987 1.9997 1.9969 1.9744 1.9329 1.9039 1.9026 1.921 ]
Position in y
[-0.0001  0.0001 -0.0734 -0.0336  0.1112  0.3188  0.5137  0.6125  0.6165  0.5565]
Type x:  <class 'numpy.ndarray'>