|
| 1 | +import numpy as np |
| 2 | +from spatialmath import SE3 |
| 3 | +import roboticstoolbox as rtb |
| 4 | +import timeit |
| 5 | +from ansitable import ANSITable, Column |
| 6 | + |
| 7 | +# change for the robot IK under test, must set: |
| 8 | +# * robot, the DHRobot object |
| 9 | +# * T, the end-effector pose |
| 10 | +# * q0, the initial joint angles for solution |
| 11 | + |
| 12 | +# Puma robot case |
| 13 | +# robot = rtb.models.DH.Puma560() |
| 14 | +# q = robot.qn |
| 15 | +# q0 = robot.qz |
| 16 | +# T = robot.fkine(q) |
| 17 | + |
| 18 | +# Panda robot case |
| 19 | +robot = rtb.models.DH.Panda() |
| 20 | +q0 = robot.qr |
| 21 | +T = SE3(0.8, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1]) |
| 22 | +# this pose does quite badly, it's from the RTB README.md |
| 23 | + |
| 24 | +# build the list of IK methods to test |
| 25 | +ikfuncs = [ |
| 26 | + robot.ikine_LM, # Levenberg-Marquadt |
| 27 | + robot.ikine_LMS, # Levenberg-Marquadt (Sugihara) |
| 28 | + robot.ikine_unc, #numerical solution with no constraints |
| 29 | + robot.ikine_con, # numerical solution with constraints |
| 30 | + robot.ikine_min # numerical solution 2 with constraints |
| 31 | +] |
| 32 | +if "ikine_a" in robot: |
| 33 | + ikfuncs.append(robot.ikine_a) # analytic solution |
| 34 | + |
| 35 | +# setup to run timeit |
| 36 | +setup = ''' |
| 37 | +from __main__ import robot, T, q0 |
| 38 | +''' |
| 39 | +N = 10 |
| 40 | + |
| 41 | +# setup results table |
| 42 | +table = ANSITable( |
| 43 | + Column("Operation", headalign="^"), |
| 44 | + Column("Time (μs)", headalign="^", fmt="{:.2f}"), |
| 45 | + Column("Error", headalign="^", fmt="{:.3g}"), |
| 46 | + border="thick") |
| 47 | + |
| 48 | +# test the IK methods |
| 49 | +for ik in ikfuncs: |
| 50 | + print('Testing:', ik.__name__) |
| 51 | + |
| 52 | + # test the method, don't pass q0 to the analytic function |
| 53 | + if ik.__name__ == "ikine_a": |
| 54 | + sol = ik(T) |
| 55 | + statement = f"sol = robot.{ik.__name__}(T)" |
| 56 | + else: |
| 57 | + sol = ik(T, q0=q0) |
| 58 | + statement = f"sol = robot.{ik.__name__}(T, q0=q0)" |
| 59 | + |
| 60 | + # print error message if there is one |
| 61 | + if not sol.success: |
| 62 | + print(' failed:', sol.reason) |
| 63 | + |
| 64 | + # evalute the error |
| 65 | + err = np.linalg.norm(T - robot.fkine(sol.q)) |
| 66 | + print(' error', err) |
| 67 | + |
| 68 | + # evaluate the execution time |
| 69 | + t = timeit.timeit(stmt=statement, setup=setup, number=N) |
| 70 | + |
| 71 | + # add it to the output table |
| 72 | + table.row(ik.__name__, t/N*1e6, err) |
| 73 | + |
| 74 | +# pretty print the results |
| 75 | +table.print() |
0 commit comments