Fix the issue with Subproblem 2 when there is a single solution (#31)

* correct the order of the input parameters to subproblem1 from subproblem2  for single solution case.

* Add a test case which verifies the fix.

* Make the test case style the same as the style of the other subproblem2 test case

---------

Co-authored-by: Burak Aksoy <burakaksoy20@gmail.com>

Author: johnwason

src/general_robotics_toolbox/general_robotics_toolbox.py

@@ -763,8 +763,8 @@ def subproblem2(p, q, k1, k2):
     if (np.abs(gamma) < eps):
         cm=np.array([k1, k2, np.cross(k1,k2)]).T
         c1 = np.dot(cm, np.hstack((a, gamma)))
-        theta2 = subproblem1(k2, p, c1)
-        theta1 = -subproblem1(k1, q, c1)
+        theta2 = subproblem1(p, c1, k2)
+        theta1 = -subproblem1(q, c1, k1)
         return [(theta1, theta2)]
 
     cm=np.array([k1, k2, np.cross(k1,k2)]).T

test/test_general_robotics_toolbox.py

@@ -329,6 +329,14 @@ def test_subproblems():
     r3=np.matmul(rox.rot(z,a3[0][0]),rox.rot(y,a3[0][1]))[:,0]        
     np.testing.assert_allclose(r3, z, atol=1e-4)
 
+    # subproblem2, another test with a random scale factor
+    scale = 0.01851852 # scale factor for random test
+    a3_2 = rox.subproblem2(scale*np.array(z), scale*np.array(x), x, y)
+    assert len(a3_2) == 1
+    
+    r3_2 = np.matmul(rox.rot(x,a3_2[0][0]),rox.rot(y,a3_2[0][1]))[:,1]
+    np.testing.assert_allclose(r3_2, y, atol=1e-4)
+    
     #subproblem3
     p4=[.5, 0, 0]
     q4=[0, .75, 0]