Repository navigation
genhexkins: take the forward's Newton step in roll, pitch and yaw rates - #4724
grandixximo wants to merge 1 commit into
Conversation
| for (ia = 0; ia < 5; ia++) | ||
| for (ib = 0; ib < 5; ib++) | ||
| for (ic = 0; ic < 3; ic++) | ||
| for (ix = 0; ix < 2; ix++) | ||
| for (iy = 0; iy < 2; iy++) | ||
| for (iz = 0; iz < 2; iz++) { | ||
| EmcPose pose = {{xy[ix], xy[iy], z[iz]}, tilt[ia], tilt[ib], turn[ic], 0, 0, 0}; | ||
| check(&pose); | ||
| } |
There was a problem hiding this comment.
That are very badly guarded nested for() loops. You should not do so. The iterations are not constants, but number of elements from arrays. You should either consistently use a define for size or a sizeof(x)/sizeof(*(x)) macro.
Also, the loop variable should be declared inside the for(). It is apparently no used anywhere else.
There was a problem hiding this comment.
Thanks, done. Every loop now has its braces, takes its bound from its array with NELEM(x) (sizeof(x) / sizeof(*(x))), and declares its variable in the for(). I split the six levels in two: rtapi_app_main() walks the orientations and check_orientation() the positions at each. The check over the pose coordinates had the same problem, so its bound and both arrays it indexes now share POSE_COORDS, and the joint buffer is EMCMOT_MAX_JOINTS instead of 6. The test still walks the same 600 poses, and without the genhexkins change it still fails 480 of them.
The forward kinematics is a Newton iteration on the platform pose that steps roll, pitch and yaw by the solution of J delta = strut errors, but it built the rotational columns of J as the strut moments (R a) x u. Those are the rates per angular velocity, which equal the rates per roll, pitch and yaw rate only at zero orientation. With the platform tilted the step was off, and the iteration converged linearly instead of quadratically. Motion runs this forward on the joint feedback every servo cycle, seeded with the pose of the cycle before. From such a seed, one cycle of motion away, on the hexapod-sim geometry tilted to 20 degrees and turned to 30, it took 12.6 iterations on average and 23 at most instead of three; tilted to 40 and turned to 60, one pose in ten did not converge within the 120 iteration limit. Running the sim through MDI tilts to A35 B35 C55, the most a feedback forward needed in the session was 117 iterations. Take the rows against the rates, w = E [dr dp dy], with the E that genhexKinematicsJacobian() uses. The same poses converge in three iterations at a quarter of the cost, none fails, and the sim session needs three at most. tests/genhexkins-forward walks 600 tilted poses with genhexkins.limit-iterations at 8, each seeded a cycle away; before this change 480 of them did not converge.
9d70099 to
54a868c
Compare
genhexkins' forward kinematics is a Newton iteration on the platform pose. It steps roll, pitch and yaw by the solution of
J delta = strut errors, but it built the rotational columns ofJas the strut moments(R a) x u. Those are the rates per angular velocity. They equal the rates per roll, pitch and yaw rate only at zero orientation. With the platform tilted the step was therefore off, and the iteration converged linearly instead of quadratically.Motion runs this forward on the joint feedback every servo cycle, seeded with the previous cycle's pose. I measured it from such a seed, one cycle of motion away, on the hexapod-sim geometry:
In the hexapod sim, through MDI tilts up to A35 B35 C55,
genhexkins.max-iterationsread 117 at the end of the session before the change, against the limit of 120, and 3 after it.The fix takes the rows against the rates,
w = E [dr dp dy], the same E thatgenhexKinematicsJacobian()already uses.tests/genhexkins-forwardwalks a grid of 600 tilted poses withgenhexkins.limit-iterationsat 8, each seeded one cycle away. Without the change, 480 of them do not converge.It applies unchanged on top of #4702, which moves genhexkins onto the parameter block, and the test passes there too.