show_dh_params.py

"""
Display DH Parameters for ThorRR Robot
"""

print('=' * 70)
print('THOR ROBOT - DH PARAMETERS')
print('=' * 70)
print()

print('LINK LENGTHS:')
print('  L1 = 202.00 mm  (Base height)')
print('  L2 = 160.00 mm  (Upper arm length)')
print('  L3 = 195.00 mm  (Forearm length)')
print('  L4 =  67.15 mm  (Wrist to TCP length)')
print()

print('=' * 70)
print('MODIFIED DH PARAMETER TABLE:')
print('=' * 70)
print()
print('Joint | theta_i           | d_i (mm) | a_i (mm) | alpha_i')
print('------|-------------------|----------|----------|----------')
print('  1   | q1                | 202.00   | 0        | +90 deg')
print('  2   | q2 - 90 deg       | 0        | 160.00   | 0 deg')
print('  3   | q3                | 0        | 0        | +90 deg')
print('  4   | q4                | 195.00   | 0        | -90 deg')
print('  5   | q5                | 0        | 0        | +90 deg')
print('  6   | q6                | 67.15    | 0        | 0 deg')
print()

print('=' * 70)
print('TRANSFORMATION MATRICES:')
print('=' * 70)
print()

print('A1 = [[c1,  0,  s1, 0  ],')
print('      [s1,  0, -c1, 0  ],')
print('      [0,   1,  0,  L1 ],')
print('      [0,   0,  0,  1  ]]')
print()

print('A2 = [[s2,  c2, 0,  L2*s2  ],    where s2=sin(q2-90), c2=cos(q2-90)')
print('      [-c2, s2, 0, -L2*c2 ],')
print('      [0,   0,  1,  0     ],')
print('      [0,   0,  0,  1     ]]')
print()

print('A3 = [[c3,  0,  s3, 0 ],')
print('      [s3,  0, -c3, 0 ],')
print('      [0,   1,  0,  0 ],')
print('      [0,   0,  0,  1 ]]')
print()

print('A4 = [[c4,  0, -s4, 0  ],')
print('      [s4,  0,  c4, 0  ],')
print('      [0,  -1,  0,  L3 ],')
print('      [0,   0,  0,  1  ]]')
print()

print('A5 = [[c5,  0,  s5, 0 ],')
print('      [s5,  0, -c5, 0 ],')
print('      [0,   1,  0,  0 ],')
print('      [0,   0,  0,  1 ]]')
print()

print('A6 = [[c6, -s6, 0, 0  ],')
print('      [s6,  c6, 0, 0  ],')
print('      [0,   0,  1, L4 ],')
print('      [0,   0,  0, 1  ]]')
print()

print('=' * 70)
print('KEY NOTES:')
print('=' * 70)
print()
print('1. Joint 2 has a -90 degree OFFSET in the transformation!')
print('   - This means q2=0 corresponds to the arm pointing HORIZONTALLY backward (-X)')
print('   - q2=90 would point vertically UP')
print()
print('2. This is the ORIGINAL ThorRR DH convention from SymPy code')
print()
print('3. At home position (all zeros):')
print('   - Arm points horizontally in -X direction (backward)')
print('   - TCP at approximately (-160, 0, 464) mm')
print()