kinematics: a Jacobian entry point, supplied by every module - #4550
Open
grandixximo wants to merge 8 commits into
Open
grandixximo wants to merge 8 commits into
grandixximo wants to merge 8 commits into
Conversation
Contributor
Author
|
Draft until #4546 merges: tests/kins-jacobian needs the fixes there (matrixkins pins at load, and the four round trips the test itself found), so CI stays red on this branch alone. Rebase and undraft once it lands. |
grandixximo
marked this pull request as draft
September 15, 2026 11:41
A feed is a speed in the work frame and the machine delivers a speed at each joint; on any kinematics but the identity the two are related by where the machine is, and nothing in the interface answered that, so a limit taken from the joints had nowhere to get it. kinematicsJacobian() answers it: jac[j][a] is how joint j responds to a unit rate of pose coordinate a, rows joints, columns in EmcPose order. It is the derivative of the inverse, since that is what every consumer multiplies by and every module has one, in joint units per pose unit so nothing is converted. A module supplies nothing: kinsJacobianFromInverse() takes central differences of its inverse, eighteen calls a pose. switchkins answers for every type, exactly for an identity type and by differences for one that registers nothing; switchkinsRegisterJacobian() takes a closed form. kinsJacobianFromMappedAxes() turns the derivative of a computed position into rows for the modules that finish in position_to_mapped_joints(). Nothing in motion calls it yet; that is the realtime seam of the limits work, and it waits on the closed forms.
Each inverse is a rotation of the pose about the table or the pivot plus offsets, so the derivative is the same rotation for the linear columns and the rotation advanced a quarter turn, times the lever, for the rotary ones. The tables and 5axiskins build the position and hand it to position_to_mapped_joints(), so they fill a matrix of the position's derivative and let kinsJacobianFromMappedAxes() place the rows, which keeps duplicate letters right. maxkins has fixed joint numbers and fills its rows directly.
…ply the Jacobian The belt sum and difference, the rotation and its quarter turn, the polar radius and angle, the calibration matrix itself, and the two templates' joint to axis assignments. These are the short ones; they are here so that no module in the tree answers by differencing when its inverse is a few lines.
The dual rotary table is its TCP inverse differentiated: the rotation matrix for the linear columns, and each term with A or B advanced a quarter turn for the rotary ones. The nutating heads are differentiated the same way, term by term through the secondary angle's r, s and t and the primary angle's sine and cosine. Their TCP inverse reads the rotary angles from the joint argument rather than from the pose, the two being the same numbers once a move is done; the derivative is taken against the pose, which is what a consumer multiplies by, and is the coupling the machine has. The TOOL type takes its angles from pins, so its inverse is linear in the pose and its rows are the coefficients.
…supply the Jacobian A strut or rod changes length by the component of its moving end's motion along it, so the rows of the parallel machines are unit vectors and moments rather than differentiated formulas. The tripod's rows are the strut directions; the linear delta's are the rod directions scaled by the rise; the rotary delta's follow from the foot staying a shin from each knee, so the foot and the knee agree along the leg. The hexapod's rows are the ones its own Newton step already builds, with the rotary columns taken through the matrix that carries roll, pitch and yaw rates to the angular velocity; with a screw lead set, whose correction is a function of the pose too, it falls back to differencing. The pentapod differentiates InvKins() the same way, in effector coordinates.
Both inverses are chains of a few closed form steps, and the derivative follows the chain: for the scara the squared reach fixes the elbow and the bearing less the outer arm's angle fixes the shoulder; for the scorbot the distance to the wrist fixes the isosceles triangle the shoulder and elbow make. Each declines at the poses where its own inverse has no derivative, the arm straight or folded.
compute_jinv() already gives radians of joint per unit of base frame twist. A pose word rate is not a twist: the roll, pitch and yaw rates reach the angular velocity through the matrix of the axes each one turns about, for the RPY convention go_rpy_mat_convert() uses. The Jacobian is that product, with the unit conversions and the unrotate coupling applied in the order the inverse applies them, and the u, v, w pass-through as ones. Having the conversion written once in the module is worth more than the closed form itself, since every consumer would otherwise guess it.
A realtime component loaded after the module under test, reaching it through the exported entry points; a failed check fails the load. Every module in the tree, every switchkins type, both direction settings on the tables. Two checks, neither reusing the module's own answer. Against the forward: perturb one joint, difference the forward, multiply by the Jacobian and expect that joint's unit vector, which catches a transposed matrix, a wrong sign, column or unit whichever way the module answered. Against the inverse: difference it here with a different step and compare entry by entry, the check for the gantry, whose forward is not one to one. Verified by mutation, one per module or shared routine, every one caught. The forward check is also a round trip of each module and found four whose forward and inverse disagreed, fixed in the commits before this one. maxkins, which disagrees away from c = 0 and u = 0, is checked against its inverse; the nutating heads read their angles from the inverse's joint argument, so they are checked against the forward.
grandixximo
force-pushed
the
kins-jacobian
branch
from
September 17, 2026 13:02
c0d40fd to
c15fdec
Compare
grandixximo
marked this pull request as ready for review
September 17, 2026 13:05
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
A feed is a speed in the work frame and the machine delivers a speed at each joint; on any kinematics but the identity the two are related by where the machine is, and nothing in the interface answered that. kinematicsJacobian() does: jac[j][a] is how joint j responds to a unit rate of pose coordinate a, the derivative of the inverse, in joint units per pose unit. A module that supplies nothing is answered by central differences of its inverse; switchkins answers per type, exactly for an identity type. Nothing in motion calls it yet; that is the realtime seam of the limits work and waits for the closed forms to be in.
Every module in the tree supplies a closed form, one commit per shape: the tables and heads (the rotation for the linear columns, the rotation a quarter turn on times the lever for the rotary ones), the short linear maps, the dual rotary table and the nutating heads term by term, the parallel machines as strut directions and moments, the arms along their chains, genser from its geometric Jacobian with the RPY rate conversion written once in the module.
tests/kins-jacobian loads a checker after each module and runs two checks that do not reuse the module's answer: perturb a joint, difference the forward, multiply by the Jacobian and expect that joint's unit vector; and difference the inverse with another step and compare entry by entry. Every module, every switchkins type, both direction settings on the tables. Verified by mutation, one per module, every one caught.
The forward check is also a round trip of each module, and it is what found the fixes in #4546; the test needs those, and matrixkins' pins existing at load, so this comes after that PR. Part of the plan in #4374.