Skip to content

Commit 6160566

Browse files
committed
put angles into range [-pi,pi)
update tests
1 parent 2c11715 commit 6160566

File tree

2 files changed

+10
-5
lines changed

2 files changed

+10
-5
lines changed

roboticstoolbox/models/DH/Puma560.py

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
import numpy as np
2020
from roboticstoolbox import DHRobot, RevoluteDH
2121
from spatialmath import SE3
22+
from spatialmath import base
2223

2324

2425
class Puma560(DHRobot):
@@ -260,6 +261,7 @@ def ik3(robot, T, config='lun'):
260261
else:
261262
raise ValueError('bad configuration string')
262263

264+
263265
# Solve for theta[1]
264266
# V114 is defined in equation 43, p.39.
265267
# r is defined in equation 47, p.39.
@@ -295,8 +297,9 @@ def ik3(robot, T, config='lun'):
295297
den = np.cos(theta[1]) * Pz - np.sin(theta[1]) * V114
296298
theta[2] = np.arctan2(a3, d4) - np.arctan2(num, den)
297299

298-
return theta
300+
theta = base.angdiff(theta)
299301

302+
return theta
300303

301304
return self.ikine_6s(T, config, ik3)
302305

@@ -307,7 +310,9 @@ def ik3(robot, T, config='lun'):
307310
print(puma)
308311
print(puma.dyntable())
309312
T = puma.fkine(puma.qn)
313+
print(puma.ikine_a(T, 'lu').q)
310314
print(puma.ikine_a(T, 'ru').q)
315+
print(puma.ikine_a(T, 'ld').q)
311316
print(puma.ikine_a(T, 'rd').q)
312-
qi = puma.ikine_a(T, 'rd').q
313-
puma.plot(qi)
317+
318+
puma.plot(puma.qz)

tests/test_DHRobot.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -894,12 +894,12 @@ def test_ikine_a(self):
894894
sol = puma.ikine_a(T, "u")
895895
self.assertTrue(sol.success)
896896
self.assertAlmostEqual(np.linalg.norm(T-puma.fkine(sol.q)), 0, places=6)
897-
self.assertTrue(sol.q[1] > -3)
897+
self.assertTrue(sol.q[1] > 0)
898898

899899
sol = puma.ikine_a(T, "d")
900900
self.assertTrue(sol.success)
901901
self.assertAlmostEqual(np.linalg.norm(T-puma.fkine(sol.q)), 0, places=6)
902-
self.assertTrue(sol.q[1] < -3)
902+
self.assertTrue(sol.q[1] < 0)
903903

904904
sol = puma.ikine_a(T, "n")
905905
self.assertTrue(sol.success)

0 commit comments

Comments
 (0)