correct the order of the input parameters to subproblem1 from subprob…

…lem2 for single solution case.
rpiRobotics · Aug 5, 2024 · 928cc5a · 928cc5a
1 parent d0f94aa
commit 928cc5a
Showing 1 changed file with 2 additions and 2 deletions.
diff --git 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