Subproblem 2 Gives wrong results / errors for the single solution case

[burakaksoy]

In the code of Subproblem 2, for single-solution case, order of the parameters to subproblem 1 is wrong.

This is the test to verify the issue:

# subproblem2 another test
a3_2 = rox.subproblem2(z, x, x, y) # Passes (apparently by luck)
a3_22 = rox.subproblem2([0, 0, 0.01851852], [0.01851852, 0, 0], x, y) # This gives the error below

"""
Traceback (most recent call last):
File "<stdin>", line 1, in <module>
File "/home/burak/.local/lib/python3.8/site-packages/general_robotics_toolbox/general_robotics_toolbox.py", line 766, in subproblem2
    # theta1 = -subproblem1(k1, q, c1)
File "/home/burak/.local/lib/python3.8/site-packages/general_robotics_toolbox/general_robotics_toolbox.py", line 706, in subproblem1
    theta = subproblem0(epp, eqp, k)
File "/home/burak/.local/lib/python3.8/site-packages/general_robotics_toolbox/general_robotics_toolbox.py", line 660, in subproblem0
    assert (np.dot(k,p) < eps) and (np.dot(k,q) < eps), \
AssertionError: k must be perpendicular to p and q
"""

assert len(a3_2) == 1
assert len(a3_22) == 1
a3_2_check = [(0.0, 1.5707963267948966)]
np.testing.assert_allclose(a3_2, a3_2_check, atol=1e-4)
np.testing.assert_allclose(a3_22, a3_2_check, atol=1e-4)

[burakaksoy]

PR #24 resolves the issue

[johnwason]

@aelias36 this looks right to me, but I haven't studied these subproblems in detail in quite a long time. Can you confirm PR #24?

[johnwason]

The matlab toolbox has the same error. I probably copied from there first: https://github.com/rpiRobotics/general-robotics-toolbox/blob/master/subproblem2.m

I did not write the matlab toolbox.