Repository navigation
scarakins, three21kins: keep joints within a half turn of the previous value - #4867
Conversation
| /* 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; |
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
296d694 to
088b029
Compare
088b029 to
02bb043
Compare
| @@ -45,6 +45,12 @@ static double angleDiff(double a, double b) | |||
| return d; | |||
| } | |||
There was a problem hiding this comment.
@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.
There was a problem hiding this comment.
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_PIconverting 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.
There was a problem hiding this comment.
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.
|
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>
06edc4a to
a10b813
Compare
|
Squashed. |
#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:
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.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 ofnear. pumakins uses it too, in place of its round trip through radians, and pumakins and three21kins compare their branch flags with it, in place ofangleDiff().tests/kins-unwrapwalks each module's joints across ±180 the way motion calls the kinematics. It takes overpumakins-unwrap, whose four walks move unchanged intokins-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 awalksfile naming its module, and each module is its own directory, as int0andio-startup, soruntests -jruns 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