Skip to main content

Custom Kinematics

See the CustomKinematics_Implementation.project sample project and the CustomKinematics.library library in the installation directory of CODESYS under ..\CODESYS SoftMotion\Examples.

This example describes how to create a library with a custom kinematic transformation (Gantry3C) and how to use this library in a project to control the robot.

The 'Gantry3C' kinematic transformation consists of 3 linear axes (X, Y, and Z) which carry a tool head. The tool head consists of an extra axis which carries a mounted tool. The tool head can be turned around the Z-axis.

_sm_img_custom_kinematik.png

1. Create a new library project.

Create a new project with the Empty library template.

_sm_img_create_empty_project.png

2. Add a Library Manager.

Add a Library Manager to the POUs view. Add the SM3_Transformation, SM3_Math, and SM3_Error libraries to the Library Manager.

3. Create the Gantry3C function block.

This function block implements the interfaces MC_KIN_REF_SM3 and ISMKinematicsWithInfo2 from the library SM3_Transformation. You can define an offset as input for each axis. This offset is subtracted before the forward transformation and added after the inverse transformation.

_sm_img_fb_gantry3c.png

Example of Gantry3C with two decoupled kinematics and KinCoupled:

FUNCTION_BLOCK Custom_Kin_Gantry3 IMPLEMENTS ISMPositionKinematics
FUNCTION_BLOCK Custom_Kin_CAxis IMPLEMENTS ISMOrientationKinematics

FUNCTION_BLOCK Custom_Kin_Gantry3C EXTENDS Kin_Coupled

Provide the function blocks Custom_Kin_Gantry3 and Custom_Kin_CAxis as inputs for the Kin_Coupled function block during initialization. Now Custom_Kin_Gantry3C becomes a coupled kinematics combining the position and orientation kinematics.

4. Implement the methods of the MC_KIN_REF_SM3 interface and the NumAxes4 property.

AxesToCartesian: Forward kinematics: Calculation of the position and orientation from the axis values.

CartesianToAxes: Inverse kinematics: Calculation of the axis values from position and orientation.

NumAxes: Number of axes of the kinematics

_sm_img_cartesian_to_axes.png
_sm_img_axes_to_cartesian.png
_sm_img_gantry3c_methods.png

5. Implement the methods of the ISMKinematicsWithInfo2 and ISMKinematicsWithInfo interfaces.

GetAxisProperties: Properties, such as type of axis. The limits for each axis can be defined.

GetKinematicsName: Name of the kinematics

IsSingularity: Can be ignored for this kinematics

_sm_img_get_axis_properties.png
_sm_img_get_kinematics_name.png

6. Specify the project information.

_sm_img_project_information.png

7. Check the library for compiler errors.

To check the library, click Build → Check all Pool Objects.

8. Create the description in the axis group configurator.

When the POU has the attribute sm_kin_libdoc, the comment specified in the function block is used in the axis group configurator as a description of the kinematics. Restructured text formatting is used for this.

_sm_img_restructured_text.png
_sm_img_axis_group_description.png
_sm_img_axis_group_configurator.png
Procedure. To generate the description of the kinematics from the function block comments in the axis group editor, follow these steps:

  1. Include the attribute sm_kin_libdoc as shown in the image above.

  2. Include the required project properties DocFormat and DocLanguages (in the project information dialog) related to the library documentation.

    Result:

    _sm_img_project_information_libdoc.png

  3. Make sure that the installation directory of CODESYS contains the libdoc.exe executable file (directory: ...\DocScripting\3.5.xx.0\).

  4. Download the 32-bit wkhtmltox converter with version 0.12.5 or lower from https://wkhtmltopdf.org/downloads.html. Extract the wkhtmltox.dll file and copy it to ...\DocScripting\3.5.xx.0\bin in the CODESYS installation directory.

    Important

    Since wkhtmltox.dll no longer supports images by default in Version 0.12.6 and higher, you need to use Version 0.12.5 or lower.

  5. Create the system variable LIBDOC_CODESYS in Windows. Set the value to the file path of CODESYS.exe. Make sure to specify the path name and the profile name in double quotation marks.

    Result:

    _sm_img_libdoc_path.png

  6. Save, close, and reopen the library project.

  7. Click File → Save Project as Compiled Library.

    The compiled library will be installed to the project which uses this kinematics.

Using the library in a project (CustomKinematics_Implementation.project)

The project demonstrates how to use and control the kinematics created in the previous section by jogging or commanding a movement.

  1. Create a CODESYS standard project with the CODESYS SoftMotion Win controller.

  2. Open the Library Manager and install the library CustomKinematics.compiled-library (created in the previous section) containing the kinematics Gantry3C. Add the library to the Library Manager.

  3. Add an axis group object and select the KinCustom.Gantry3C kinematics.

    Result:

    _sm_img_select_kinematics.png

  4. Add 4 virtual axes to the SoftMotion General Axis Pool axis pool in the device tree. (DriveX, Y, Z, and C)

  5. Open the axis group configurator. In the Mapping to Axes section, map the axes to their respective fields.

    Result:

    _sm_img_axis_mapping.png

  6. The program Prg_Visu is the main program that controls the axis group. When the application is started, Prg_Visu powers up the axis group and enables it. Moreover, it calls all necessary function blocks to control the axis group (jogging, linear and PTP movements). All of these movements can be controlled from the attached visualization.

    Result:

    _sm_img_prg_visu.png

  7. Function block: Jog: This FB combines jogging in axis space as well as in Cartesian space. It uses two instances of SMC_GroupJog2. One is for axis space and the other is for Cartesian space. This function block can be used with the visualization templates V_RobotPosition_axes and V_RobotPosition_cart_Jog to jog any kinematics in Cartesian and axis space.

    Result:

    _sm_img_fb_jog.png
    _sm_img_fb_jog2.png

  8. FB SMC_TRAFOF_Gantry3C: This function block transforms the input axis values of a Gantry3C kinematics to its respective TCP values. In addition, the axis values are normalized so that they can be used with the visualization template TRAFOF_Gantry3C_YZ to visualize the movements of the kinematics.

    Result:

    _sm_img_trafof_gantry3c.png

  9. Compile and run the application. Open the visualization. You can jog the robot in axis space as well as in Cartesian space. There are also visualization templates to command a linear or a PTP movement.

    Result:

    _sm_img_custom_kinematics_visu.png

Tip

The example described here discusses the positioning and orientation axes in a common function block. Many kinematics can comprise two decoupled partial kinematics: one positioning kinematic (delta, gantry, etc.) and one orientation kinematics (tools like C-axis, Wrist2, Wrist3, etc.). Both kinematics are connected to each other at the "flange point", the TCP of the positioning kinematics. The orientation kinematics is characterized by the fact that it is able to calculate the vector from the flange point to the TCP of the coupled kinematics. The calculation is done using only the orientation of this TCP (meaning that it is independent of the positioning kinematics or independent of the orientation of the flange point). The positioning kinematics in turn has to be able to determine its axis positions from only the position of the flange point. It must not depend on the orientation of the flange point.

In this case, you can resort to interfaces such as ISMPositionKinematics or ISMOrientationKinematics. To implement these interfaces, define one function block to implement ISMPositionKinematics and another function block to implement ISMOrientationKinematics. Finally, define a function block that extends the function block Kin_Coupled (from SM3_Transformation) with the previously defined function blocks as inputs.

For more information, see: Creating Custom Kinematics

: