pumakinsInverse() returns joints 4 and 6 straight from atan2() (src/emc/kinematics/pumakins.c, lines 347 and 372 at 41568b4), so they always land in (-180, 180], plus 180 after a wrist flip. The joints passed in are only used at the singular pose (line 344), not to pick the nearest whole turn.
When the wrist crosses ±180 the commanded joint jumps a full turn in one servo cycle. It shows with every trajectory planner, since they all take what the inverse returns.
Fix: bring th4 and th6 within 180 degrees of the incoming joint[3] and joint[5], and add a test that crosses ±180 on both joints.
pumakinsInverse()returns joints 4 and 6 straight fromatan2()(src/emc/kinematics/pumakins.c, lines 347 and 372 at 41568b4), so they always land in (-180, 180], plus 180 after a wrist flip. The joints passed in are only used at the singular pose (line 344), not to pick the nearest whole turn.When the wrist crosses ±180 the commanded joint jumps a full turn in one servo cycle. It shows with every trajectory planner, since they all take what the inverse returns.
Fix: bring th4 and th6 within 180 degrees of the incoming
joint[3]andjoint[5], and add a test that crosses ±180 on both joints.