Skip to content

scarakins, three21kins: keep joints within a half turn of the previous value - #4867

Merged
grandixximo merged 1 commit into
LinuxCNC:masterfrom
Ash-Pera:kins-unwrap
Nov 21, 2026
Merged

grandixximo merged 1 commit into
LinuxCNC:masterfrom
Ash-Pera:kins-unwrap

Conversation

@Ash-Pera

@Ash-Pera Ash-Pera commented Nov 19, 2026 •

Copy link
Copy Markdown
Contributor

#4708 made pumakins return each joint within a half turn of the joint motion hands it. Two modules have the same problem and didn't get that change:

  • scarakins: joint 0 is the difference of two atan2() results, so it comes back a whole turn off (and joint 3 a turn the other way) wherever the end effector's bearing crosses ±180. The scara sims allow joint 0 ±360°, so they reach it.
  • three21kins: carries pumakins' solver, so the base (with the shoulder flag set) comes back one or two turns off, and the wrist past ±180° a whole turn off.

Both now do what pumakins does, with a static nearestTurn(angle, near) in each module that moves an angle in degrees by whole turns to within a half turn of near. pumakins uses it too, in place of its round trip through radians, and pumakins and three21kins compare their branch flags with it, in place of angleDiff().

tests/kins-unwrap walks each module's joints across ±180 the way motion calls the kinematics. It takes over pumakins-unwrap, whose four walks move unchanged into kins-unwrap/pumakins/walks, and adds scarakins (joint 0 with the elbow bent each way) and three21kins (pumakins' walks). One component and one shared script serve every module, each line of a walks file naming its module, and each module is its own directory, as in t0 and io-startup, so runtests -j runs them in parallel. Without the unwrap in each module every walk fails, at 14 to 50 of its 50 poses; with it all hold.

One behaviour change to note: as with pumakins since #4708, motion's endpoint checks (inRange()) and joint-interpolated moves (inverse_settled()) now get the joint value closest to the one queued before, so near a joint limit a move the old answer accepted can be refused, or the reverse.

🤖 Generated with Claude Code

Comment thread src/emc/kinematics/three21kins.c Outdated
Comment on lines 373 to 383
/* keep each joint within a half turn of the one passed in, as pumakins */
th1 = joint[0]*PM_PI/180 + angleDiff(th1, joint[0]*PM_PI/180);
th2 = joint[1]*PM_PI/180 + angleDiff(th2, joint[1]*PM_PI/180);
th3 = joint[2]*PM_PI/180 + angleDiff(th3, joint[2]*PM_PI/180);
th4 = joint[3]*PM_PI/180 + angleDiff(th4, joint[3]*PM_PI/180);
th5 = joint[4]*PM_PI/180 + angleDiff(th5, joint[4]*PM_PI/180);
th6 = joint[5]*PM_PI/180 + angleDiff(th6, joint[5]*PM_PI/180);

/* copy out */
joint[0] = th1*180/PM_PI;
joint[1] = th2*180/PM_PI;

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why is there a conversion from degrees to radians and reverse? Can't this be done in one single unit?

BTW, pumakins seems to do the same, but that should get the same question. Also, that would make this PR a computer generated version of copy-paste and intelligence sidelined?

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fair point, and it applies to pumakins too. The joints come in and go out in degrees, so the unwrap is now done there, once, by a new kinsNearestTurn(angle, near) in kins_util, which every module already links, used by pumakins, three21kins and scarakins. pumakins and three21kins still each have a private radian angleDiff() for their flag checks; those could move to kins_util later.

On copy-paste: yes, three21kins carries pumakins' solver, and I copied #4708's lines to keep the two identical, radian round trip included, which was worth fixing instead. Written with Claude, per the commit trailers; each test fails without its fix.

Comment thread tests/kins-unwrap/test.sh Resolved
Comment thread src/emc/kinematics/kins_util.c Resolved
Comment thread src/emc/kinematics/kins_util.c Resolved
Comment thread src/emc/kinematics/pumakins.c Resolved
Comment thread src/emc/kinematics/three21kins.c Resolved
Comment thread src/emc/kinematics/scarakins.c Resolved
Comment thread tests/kins-unwrap/shared-test.sh Resolved
Comment thread tests/kins-unwrap/shared-test.sh Resolved
Comment thread src/emc/kinematics/pumakins.c Outdated
Comment on lines 40 to 46
@@ -45,6 +45,12 @@ static double angleDiff(double a, double b)
return d;
}

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@grandixximo please have a look too...

I know the functions are not the same, but wouldn't it be possible to change the three remaining uses of angleDiff() with nearestTurn() using some simple algorithmic tweak? You convert the first arg to radians, whereas the other function would require the second to be converted into degrees (a one conversion vs one conversion issue). Getting rid of the while construct would be a preference in either case.

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes. nearestTurn(x, j) - j is x - j brought within a half turn, so each flag check can be

fabs(nearestTurn(X*180.0/PM_PI, joint[N]) - joint[N]) < FLAG_FUZZ*180.0/PM_PI

converting the atan2() expression X instead of the joint, and angleDiff() with its while loops goes, in three21kins as well. I tested it on top of this PR: the results are identical.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done, in grandixximo's form: each flag check is fabs(nearestTurn(X*180.0/PM_PI, joint[N]) - joint[N]) < FLAG_FUZZ*180.0/PM_PI, and angleDiff() is gone from both modules. Old and new builds give the same flags over 8000 joint sets each, every branch moved by whole turns.

@BsAtHome
BsAtHome requested a review from grandixximo November 19, 2026 22:19

@grandixximo grandixximo left a comment •

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please apply the cleanup up as discussed above, thanks :-)
angleDiff() can be deleted and nearestTurn() will take its place

@BsAtHome

Copy link
Copy Markdown
Contributor

This PR needs to be squashed, then it should be ready.

Motion hands the inverse the joints of the cycle before and takes a
joint a whole turn away from them as a step.  LinuxCNC#4708 kept pumakins'
joints within a half turn of those by taking each to radians,
unwrapping there, and taking it back to degrees; scarakins and
three21kins did nothing of the kind.

Add a static nearestTurn() to each module, which moves an angle in
degrees by whole turns to within a half turn of the joint passed in,
and apply it as the joints are copied out:

- pumakins: in place of the round trip through radians.  The answers
  are the same, but for rounding and the pick at exactly half a turn,
  where either is right.
- scarakins: to joint 0, which came from the difference of two atan2()
  results and landed anywhere in (-360, 360).  Wherever the bearing
  crossed +-180 with the elbow bent, or joint 0 was past a half turn,
  joint 0 came back a turn away and joint 3 a turn the other way.
  Joint 1 comes from acos() and needs nothing.
- three21kins: to every joint.  It carries pumakins' solver; with the
  shoulder flag set the base could come back one or two turns off, and
  the wrist roll and twist a turn off past +-180.

pumakins and three21kins also set their branch flags by comparing a
joint with the angle the inverse's formula gives, through a radian
angleDiff() that wrapped with while loops.  They compare with
nearestTurn() instead, in degrees, and angleDiff() is gone.  The flags
are unchanged.

As with pumakins since LinuxCNC#4708, motion's endpoint checks and
joint-interpolated moves now get the joint closest to the one queued
before, so near a limit a move the old answer accepted can be refused,
or the reverse.

tests/pumakins-unwrap becomes tests/kins-unwrap, one component and one
script with a directory per module, so runtests -j runs them side by
side.  Each walks the joints across +-180 the way motion calls the
kinematics and requires the joints that made each pose back: pumakins'
four walks unchanged, scarakins joint 0 with the elbow bent each way,
three21kins pumakins' walks.  Without the unwrap every walk fails, at
14 to 50 of its 50 poses; with it all hold.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
@Ash-Pera

Copy link
Copy Markdown
Contributor Author

Squashed.

@grandixximo
grandixximo merged commit 7bf07c2 into LinuxCNC:master Nov 21, 2026
19 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants