From 928cc5a6191330043797379491aec01cd48b3464 Mon Sep 17 00:00:00 2001 From: Burak Aksoy Date: Mon, 5 Aug 2024 08:01:38 -0400 Subject: [PATCH 1/3] correct the order of the input parameters to subproblem1 from subproblem2 for single solution case. --- src/general_robotics_toolbox/general_robotics_toolbox.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/general_robotics_toolbox/general_robotics_toolbox.py b/src/general_robotics_toolbox/general_robotics_toolbox.py index bea5303..65d848c 100644 --- a/src/general_robotics_toolbox/general_robotics_toolbox.py +++ b/src/general_robotics_toolbox/general_robotics_toolbox.py @@ -762,8 +762,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 From 529e556adadb9d84e53307652d85cbe87d7a5312 Mon Sep 17 00:00:00 2001 From: Burak Aksoy Date: Tue, 6 Aug 2024 11:19:39 -0400 Subject: [PATCH 2/3] Add a test case which verifies the fix. --- test/test_general_robotics_toolbox.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/test/test_general_robotics_toolbox.py b/test/test_general_robotics_toolbox.py index a280621..328603d 100755 --- a/test/test_general_robotics_toolbox.py +++ b/test/test_general_robotics_toolbox.py @@ -329,6 +329,11 @@ 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) + a3_2 = rox.subproblem2([0, 0, 0.01851852], [0.01851852, 0, 0], x, y) + assert len(a3_2) == 1 + a3_2_check = [(0.0, 1.5707963267948966)] + np.testing.assert_allclose(a3_2, a3_2_check, atol=1e-4) + #subproblem3 p4=[.5, 0, 0] q4=[0, .75, 0] From 6ce54fe20aaf26544dbcd19fa4a98d79a2800d6e Mon Sep 17 00:00:00 2001 From: Burak Aksoy Date: Tue, 6 Aug 2024 14:06:06 -0400 Subject: [PATCH 3/3] Make the test case style the same as the style of the other subproblem2 test case --- test/test_general_robotics_toolbox.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/test/test_general_robotics_toolbox.py b/test/test_general_robotics_toolbox.py index 328603d..c98d593 100755 --- a/test/test_general_robotics_toolbox.py +++ b/test/test_general_robotics_toolbox.py @@ -329,10 +329,13 @@ 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) - a3_2 = rox.subproblem2([0, 0, 0.01851852], [0.01851852, 0, 0], x, y) + # 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 - a3_2_check = [(0.0, 1.5707963267948966)] - np.testing.assert_allclose(a3_2, a3_2_check, atol=1e-4) + + 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]