|
| 1 | +#!/usr/bin/env python |
| 2 | +"""Root-cause diagnostic for the Robot.rne() vs DHRobot.rne_python() divergence |
| 3 | +originally seen in rne_compare.py -- see rne.md (issue 6) for the full writeup. |
| 4 | +
|
| 5 | +Uses a single-link revolute robot with a pure axis twist (alpha != 0, a=d=0) |
| 6 | +to isolate the frame-convention question from friction/armature/multi-link |
| 7 | +recursion complexity. Ground truth is obtained *independently* of either RNE |
| 8 | +implementation via the Lagrangian identity: for a static pose (qd=qdd=0), the |
| 9 | +required joint torque equals dV/dq, where V(q) = m g z_com(q) is the |
| 10 | +gravitational potential energy of the link's centre of mass, computed |
| 11 | +directly from link.A(q) and numerically differentiated. |
| 12 | +
|
| 13 | +Historical note: this originally showed rne_python() wrong for modified DH |
| 14 | +and Robot.rne() wrong for standard DH -- three real bugs in rne_python()'s |
| 15 | +MDH branch (see rne.md) have since been fixed, so rne_python() now agrees |
| 16 | +with ground truth for both conventions. Robot.rne()'s standard-DH case is |
| 17 | +not a bug to fix: its Featherstone recursion structurally requires the |
| 18 | +joint to be the last element of its own ETS segment, which standard DH |
| 19 | +never satisfies (the joint comes first). Robot.rne() now asserts on that |
| 20 | +case (see Robot.py) instead of silently returning a wrong answer. |
| 21 | +""" |
| 22 | + |
| 23 | +import numpy as np |
| 24 | + |
| 25 | +from roboticstoolbox import DHRobot, RevoluteDH, RevoluteMDH |
| 26 | +from roboticstoolbox.robot.Robot import Robot as RobotBase |
| 27 | + |
| 28 | + |
| 29 | +def gravity_torque_truth(robot, q0, g=9.81, h=1e-6): |
| 30 | + """Numerically-differentiated ground truth, independent of any RNE code.""" |
| 31 | + link = robot.links[0] |
| 32 | + p_local = np.array([*link.r, 1.0]) |
| 33 | + |
| 34 | + def com_z(q): |
| 35 | + A = np.asarray(link.A(q)) |
| 36 | + return (A @ p_local)[2] |
| 37 | + |
| 38 | + return g * (com_z(q0 + h) - com_z(q0 - h)) / (2 * h) |
| 39 | + |
| 40 | + |
| 41 | +def check(label, robot, q0=0.3, robot_rne_expected_to_work=True): |
| 42 | + z = np.zeros(1) |
| 43 | + q = np.array([q0]) |
| 44 | + truth = gravity_torque_truth(robot, q0) |
| 45 | + tau_py = robot.rne_python(q, z, z)[0] |
| 46 | + print(f"{label:28s} truth={truth:9.4f} rne_python={tau_py:9.4f}") |
| 47 | + print(f"{'':28s} |rne_python - truth| = {abs(tau_py - truth):.4f}") |
| 48 | + |
| 49 | + if robot_rne_expected_to_work: |
| 50 | + tau_base = RobotBase.rne(robot, q, z, z)[0] |
| 51 | + print(f"{'':28s} Robot.rne={tau_base:9.4f}") |
| 52 | + print(f"{'':28s} |Robot.rne - truth| = {abs(tau_base - truth):.4f}") |
| 53 | + else: |
| 54 | + try: |
| 55 | + RobotBase.rne(robot, q, z, z) |
| 56 | + except AssertionError: |
| 57 | + print(f"{'':28s} Robot.rne: correctly rejected (AssertionError)") |
| 58 | + else: |
| 59 | + print(f"{'':28s} Robot.rne: ERROR -- expected AssertionError, got a result") |
| 60 | + |
| 61 | + |
| 62 | +print("Single-link revolute robot, alpha=1.2 rad, a=d=0, r=[0.5,0,0], static (qd=qdd=0)") |
| 63 | +print("Ground truth = d/dq[ m g z_com(q) ], independent of both RNE implementations.") |
| 64 | +print() |
| 65 | + |
| 66 | +std = DHRobot([RevoluteDH(a=0, alpha=1.2, d=0, m=1.0, r=[0.5, 0, 0])], gravity=[0, 0, -9.81]) |
| 67 | +check("Standard DH (mdh=False)", std, robot_rne_expected_to_work=False) |
| 68 | + |
| 69 | +print() |
| 70 | +mdh = DHRobot([RevoluteMDH(a=0, alpha=1.2, d=0, m=1.0, r=[0.5, 0, 0])], gravity=[0, 0, -9.81]) |
| 71 | +check("Modified DH (mdh=True)", mdh, robot_rne_expected_to_work=True) |
| 72 | + |
| 73 | +print() |
| 74 | +print("Conclusion: rne_python is correct for both DH conventions.") |
| 75 | +print("Robot.rne is correct for modified DH, and now cleanly rejects (rather") |
| 76 | +print("than silently mis-computing) standard DH, since its Featherstone") |
| 77 | +print("recursion cannot represent standard DH's joint-first structure.") |
0 commit comments