CODESYS SoftMotion: Resolving Custom Kinematics Errors

Yes. You can implement custom CODESYS SoftMotion kinematics for a two-axis delta mechanism operating in the XZ plane. The critical requirements are a compatible kinematics function block, mutually consistent forward and inverse transformations, and a valid orientation matrix returned with every Cartesian pose. If coordinated-path processing reports smc_cp_invalid_path_elem even though independent axis moves work, validate f.mR before changing the position equations.

Define the Two-Axis Kinematic Model

A planar delta mechanism converts two rotary or linear actuator coordinates into a tool-center position in the XZ plane. Custom kinematics must provide both transformation directions:

• AxesToCartesian(a, out f) converts actuator coordinates into the Cartesian pose.
• CartesianToAxes(f, out a) converts the requested Cartesian pose back into actuator coordinates.

Keep the coordinate convention explicit. Define the X origin, Z origin, positive directions, actuator zero positions, and the physical branch represented by the inverse solution. A parallel mechanism can have multiple mathematical solutions for the same tool position. The inverse transformation must select the branch matching the installed mechanism and current operating region.

Although the robot moves only in a plane, the Cartesian pose still contains three-dimensional orientation data. Planar motion does not make the rotation matrix optional. Return a fixed valid orientation when the mechanism does not control tool rotation.

Why Coordinated Motion Rejects the Path

Successful single-axis movement proves that the drives, axes, and individual position commands can operate; it does not validate the complete Cartesian pose. Coordinated-path processing evaluates the kinematic transformation for path elements between programmed Cartesian points. It can reject a pose when position values appear correct but another required pose component is invalid.

The reported failure in this case was smc_cp_invalid_path_elem. The decisive defect was an invalid rotation matrix in f.mR, whose type is SMC_Matrix3. Correct position equations therefore did not make the path valid.

A rotation matrix must represent an orthonormal, right-handed coordinate frame. Its three row or column direction vectors—depending on the API convention—must have unit length, be mutually perpendicular, and produce a determinant of approximately +1. A zero-filled matrix, duplicated axes, non-unit vectors, or a left-handed frame is not a valid orientation.

For a tool frame aligned with the Cartesian frame, the mathematical identity matrix is the natural fixed orientation:

[1 0 0]
[0 1 0]
[0 0 1]

Map those values into SMC_Matrix3 according to the structure layout defined by the installed CODESYS libraries. If the tool frame has a fixed mounting offset, use the corresponding fixed rotation instead.

Run the Diagnostic Sequence

1. Check the implemented interface. Confirm that the custom function block implements ISMKinematicWithInfo2. An otherwise correct geometry cannot satisfy the motion subsystem if the expected interface contract is missing.
2. Test forward transformation. Supply known actuator positions to AxesToCartesian. Verify that the returned X and Z positions match independently calculated points and that all returned pose fields contain finite values.
3. Inspect f.mR. Confirm that every call assigns all rotation-matrix elements. Do not rely on default memory contents or assume that orientation is ignored for planar motion.
4. Test the round trip. For a valid actuator vector a, calculate f = AxesToCartesian(a), then aCompare = CartesianToAxes(f). Compare a with aCompare using a tolerance appropriate to the application and numeric precision.
5. Repeat across the workspace. Test random reachable positions, points near the intended operating boundary, and positions on both sides of the nominal centerline. A single center point will not reveal branch changes or singular behavior.
6. Test path interpolation. After point transformations pass, command short coordinated paths entirely inside the verified workspace. This separates pose-format defects from reachability and interpolation problems.

Implement the Corrective Changes

Initialize the complete Cartesian result on every execution of AxesToCartesian. Calculate the XZ position, assign any unused Cartesian position component according to the chosen planar convention, and explicitly populate f.mR with a valid rotation. Keeping orientation constant is appropriate when the mechanism supplies translation only.

Then make the inverse transformation honor the same frame definitions. The inverse function must interpret the Cartesian pose using the same origin, scale, axis directions, and tool-frame convention used by the forward function. Reject unreachable targets deliberately rather than allowing invalid square roots, divisions by values approaching zero, or non-finite axis results to propagate into path processing.

Handle multiple inverse solutions deterministically. Select the mechanically valid elbow or linkage configuration and prevent an unexpected branch switch between adjacent Cartesian points. A discontinuous axis solution can turn a smooth Cartesian segment into a large actuator jump.

CODESYS documentation includes a sample project for a simple custom robotics kinematic. Use it to compare the required interface structure and pose handling, then replace its geometry with the equations for the two-axis delta mechanism.

Verify Geometry, Orientation, and Continuity

Use the same numeric tolerance for repeatable comparisons, but choose it from the controller precision, axis scaling, mechanical resolution, and required positioning accuracy. Do not require exact floating-point equality. Log the input axes, Cartesian result, rotation matrix, inverse result, and failure status for the first rejected sample; this usually identifies whether the defect is geometric, numeric, or structural.

Avoid Recurring Custom-Kinematics Pitfalls

• Leaving orientation uninitialized: A planar robot still needs a valid three-dimensional pose representation.
• Testing only independent axes: Single-axis commands bypass important coordinated-path and Cartesian-pose checks.
• Checking only one pose: Center-workspace tests can hide unreachable regions, singularities, and inverse-branch changes.
• Using exact equality: Forward and inverse trigonometric calculations require tolerance-based comparison.
• Mixing frames: Different origins, signs, units, or tool offsets between transformation directions break round-trip consistency.
• Returning non-finite values: Guard geometric domain limits and denominators before returning a pose or actuator solution.
• Changing inverse branches: Choose one mechanically permitted configuration and maintain continuity along the commanded path.

FAQ