Skip to content

Add three21kins analytical kinematics solver - #4270

Open
sleepybishop wants to merge 1 commit into
LinuxCNC:masterfrom
sleepybishop:three21
Open

Add three21kins analytical kinematics solver#4270
sleepybishop wants to merge 1 commit into
LinuxCNC:masterfrom
sleepybishop:three21

Conversation

@sleepybishop

@sleepybishop sleepybishop commented Jul 26, 2026

Copy link
Copy Markdown

Hello! This PR ports the three21kins kinematics solver from gomotion into LinuxCNC

There is a need for a simpler and more flexible kinematics solver in LinuxCNC for robot arms:

  • The existing genserkins module is generalized, but it can be complex and slow.
  • Conversely, pumakins is not very flexible, and writing custom kinematics modules for each new type of robot is quite tedious.

To bridge this gap, three21kins was ported from the gomotion repository, just like our other kinematics modules. I'm hopeful it would be of use to many 6-DOF arm + wrist style robot arms.

As a practical example, I have already switched my own robot arm to use this solver.

Please let me know if you have any feedback or suggestions!

@sleepybishop
sleepybishop marked this pull request as ready for review July 26, 2026 21:59
@grandixximo

Copy link
Copy Markdown
Contributor

Leaving it here for visibility, looks good to me, thank you

@grandixximo

grandixximo commented Jul 28, 2026

Copy link
Copy Markdown
Contributor

Two pieces of feedback looking at it again:

Docs: New kinematics modules are expected to show up in a few places; pumakins is a good template since three21kins follows the same pattern:

  • A *loadrt three21kins* line in the SYNOPSIS and a === three21kins section in docs/src/man/man9/kins.9.adoc, describing the HAL pins (A1..A3, D1..D4, D6). Without this, users have to read the source to learn the pin names.
  • A row in the kinematics table in docs/src/hal/components.adoc.
  • An entry in the supported-modules list in docs/src/motion/switchkins.adoc, since the module implements the full switchkins setup.

NaN on unreachable targets (optional, non-blocking): In the inverse kinematics, both sqrt(sumSq) and sqrt(A3*A3 + D4*D4 - k*k) can receive negative arguments when the commanded pose is unreachable (or the wrist center falls inside the d23 cylinder). The result is NaN joint values returned with success status, and nothing upstream checks for NaN. Some hardening would be nice, for example clamping the sqrt arguments to >= 0 or returning nonzero so motion can report an inverse kinematics failure. I want to be clear that pumakins has the identical unguarded code and the gomotion original does too, so this is inherited behavior and in no way a blocker for this PR; consider it a suggestion for a follow-up improvement, possibly shared with pumakins.

#include <hal.h>
#include <kinematics.h>

#include "three21kins.h"

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.

Is there a reason why you need to have the include file? It seems only used by this kinematics. You can just add the defined at the top here.

Comment on lines +14 to +21
#define THREE21_A1 (*(haldata->a1))
#define THREE21_A2 (*(haldata->a2))
#define THREE21_A3 (*(haldata->a3))
#define THREE21_D1 (*(haldata->d1))
#define THREE21_D2 (*(haldata->d2))
#define THREE21_D3 (*(haldata->d3))
#define THREE21_D4 (*(haldata->d4))
#define THREE21_D6 (*(haldata->d6))

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.

You are using these several times in the code below. Reading these values is expensive because they are volatile.

Having these at the right time as a local version is cheaper and better for the compiler to optimize. But, you should be smart when and where which variables are required.

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