概述
基于SoftMotion规范继承一系列接口，实现能够被AxisGroup对象加载的机构运动学模型，目标为计算机器人各个关节的运动方式，以确保工具尖端——工具中心点 (TCP)——能够以期望的姿态跟随指令路径运动。运动学或运动学变换是此计算的基础。
包含以下产品组件：
SF512x_Kinematics：产品SF512x的主库，加载主库即可在AxisGroup中选择内部运动学变换模型。
下表概述了各个产品组件
| 产品组件 | 描述说明 |
| SF512x_Kinematics | 产品SF512x的主库，加载主库即可在AxisGroup中选择内部运动学变换模型。 |
安装与卸载
安装
1.1安装要求
建议平台版本：CODESYS大于CODESYS V3.5 SP20 Patch6+。
建议平台库文件：CODESYS Math Libraries。
辅助组件：确保调用程序已加载Licensing组件。
RTE组件：确保控制器RTE已预载Licensing组件。
控制器硬件：中科时代出品系列控制器。
1.2安装过程
在任意一个已打开的工程中，打开库管理器页面:
点击按钮：
点击：选择个人路径下的SF512x_Kinematics_v3p5_sp20_patch6+_v2.0.1.1.compiled-library（产品主库文件），进行安装，完成之后点击：
回到界面，点击，进行查找SF512x_Kinematics：
双击安装即可完成在项目中的安装。
更新
位于界面
点击：
双击包含版本号的中间列以选择安装的最新/其他版本。
卸载
位于界面，查找SF512x_Kinematics关键字，选择需要卸载的版本，点击卸载：
技术说明
快速启动
1.1本例软、硬件配置
| 硬件：中科时代出品系列控制器 | 软件：SF512x_Kinematics |
1.2 实验原理图
Kinematics运动学变换基础实现接口为AxesToCartesian和CartesianToAxes，数学表现为机器人轴坐标系和笛卡尔坐标系间的正-反映射。下为简单图解：
1.3 实验步骤
工程环境准备：基于章节二.安装与卸载正确安装主库与相关依赖，确保与使用的中科时代控制器存在真实物理连接，且处于同一网段下。
新建工程，双击选定页面如下：
扫描网络并且测试与控制器硬件的连接，以进行下一步测试。
右键单击，创建对象。
左键单击-，选择，以该Scara机器人模型为例。
填入机器人构型参数和产品授权码，绑定虚拟轴对象/实际轴对象。
接下来便可以基于该对象，测试相关的库相关功能，以为例，在可视化界面中添加该控件，指定该对象，可直接控制虚拟轴/实际轴进行运动。
功能介绍
SoftMotion接口支持清单
SF512x_Kinematics 当前已对接以下 CODESYS SoftMotion 接口（TRAFO 命名空间）：
| 接口 | 支持状态 | 接口意义 |
| TRAFO.MC_KIN_REF_SM3 | 已支持 | 运动学基础引用接口，提供运动学对象基础能力 |
| TRAFO.ISMKinematicWithInfo2 | 已支持 | 运动学信息扩展接口，提供轴数量、轴属性与姿态空间信息 |
| TRAFO.ISMKinematicWithInitialization | 已支持 | 初始化接口，定义初始化生命周期与状态可用性 |
| TRAFO.ISMPositionKinematics_Offset2 | 已支持 | 带工具偏移的位置运动学接口，支持偏移场景正逆解 |
| TRAFO.ISMKinematicWithConfigurations2 | 已支持 | 构型管理接口，支持构型数据尺寸、默认值与连续性判定 |
参数初始化
示例：以SCARA模型为例的子类接口映射：
机器人结构说明如下：
| 关节 | 类型 | 正方向 | 说明 |
| A1 | 旋转关节 | 垂直基座向上（右手定则） | 第一臂绕基座Z轴旋转 |
| A2 | 旋转关节 | 垂直基座向上（右手定则） | 第二臂绕A1末端Z轴旋转 |
| A3 | 直线关节 | 向上为正 | TCP沿Z轴上下平移 |
| A4 | 螺旋关节 | 向上为正 | 旋转同时引起TCP沿Z轴移动（ZC耦合） |
运动学零位定义：两个活动连杆成一条直线，第一个活动连杆沿基座对称线方向（即MCS坐标系的X轴正方向）。Z轴垂直基座向上，Y轴由右手定则确定。
ZC耦合（ZCcoupled）特性：A4关节旋转时，TCP在绕Z轴旋转的同时会沿Z轴产生线性位移，该位移量由参数 dPitchZC（每转一圈的移动量，单位mm）决定。
下表给出接口在“以SCARA模型为例”的子类中的参数映射关系，用于说明接口定义在工程中的落地形式。
| 参数名 | 类型 | 单位 | 范围 | 说明 |
| dOffsetA1 | LREAL | ° | [-360, 360] | A1轴运动学零位时的关节角度偏移 |
| dLowerA1 | LREAL | ° | [-360, 360] | A1轴运动范围下限 |
| dUpperA1 | LREAL | ° | [-360, 360] | A1轴运动范围上限 |
| dOffsetA2 | LREAL | ° | [-360, 360] | A2轴运动学零位时的关节角度偏移 |
| dLowerA2 | LREAL | ° | [-360, 360] | A2轴运动范围下限 |
| dUpperA2 | LREAL | ° | [-360, 360] | A2轴运动范围上限 |
| dOffsetA3 | LREAL | mm | [-400, 400] | A3轴运动学零位时的关节移动距离偏移 |
| dLowerA3 | LREAL | mm | [-400, 400] | A3轴运动范围下限 |
| dUpperA3 | LREAL | mm | [-400, 400] | A3轴运动范围上限 |
| dOffsetA4 | LREAL | ° | [-720, 720] | A4轴运动学零位时的关节角度偏移 |
| dLowerA4 | LREAL | ° | [-720, 720] | A4轴运动范围下限 |
| dUpperA4 | LREAL | ° | [-720, 720] | A4轴运动范围上限 |
| dLinkLength1 | LREAL | mm | [0, 800] | 连杆1长度（关节1与关节2轴线的公垂线长度） |
| dLinkLength2 | LREAL | mm | [0, 800] | 连杆2长度（关节2与关节3轴线的公垂线长度） |
| dPitchZC | LREAL | mm | [-200, 200] | A4轴正转360°时TCP沿Z轴的移动量（向下移动为负值） |
| sLicenseCode | STRING(255) | — | — | 产品授权码，用于许可证验证 |
方法列表
Initialize
返回值类型: TRAFO.SMC_Error
功能：执行运动学功能块的初始化，在调用任何其他方法之前必须先成功调用此方法。
说明： 1. 完成参数装载与合法性校验； 2. 完成许可证或运行前置条件校验； 3. 初始化运行状态并写入“已初始化”标志； 4. 返回标准错误码，保证上层可判定失败原因。
返回值为 SMC_NO_ERROR — 初始化成功；SMC_TRAFO_NOT_INITIALIZED — 参数无效或授权失败
IsInitialized
返回值类型: BOOL
功能：查询运动学功能块是否已成功初始化。
说明：此方法是唯一可在 Initialize 调用之前安全调用的方法，返回内部标志 isInit 的状态。当 Initialize 成功执行后返回 TRUE，否则返回 FALSE。
NumAxes（属性）
返回值类型: UDINT
功能：返回该运动学模型的轴数量。以Scara机器人为例，固定返回 4，对应 A1、A2、A3、A4 四个关节轴。
GetKinematicsName
返回值类型: STRING(255)
功能：返回该运动学模型的可读名称字符串，用于标识和调试。以SCARA模型为例，固定返回字符串 'Kin_Scara4_ZCcoupled'。
GetAxisProperties
输入类型: udiAxis (UDINT) — 轴编号 [0..NumAxes-1] 输入输出(In_Out): props (TRAFO.Axis_Properties) — 轴属性结构体输出
功能：返回指定轴的属性信息，包括轴类型（旋转/直线）和运动范围限位。CODESYS SoftMotion 通过此方法获取各轴的运动学约束。
以SCARA模型为例：各轴属性：
| 轴编号 | udiAxis | 类型 | 限位来源 |
| A1 | 0 | Rotary（旋转） | dLowerA1 / dUpperA1 |
| A2 | 1 | Rotary（旋转） | dLowerA2 / dUpperA2 |
| A3 | 2 | Linear（直线） | dLowerA3 / dUpperA3 |
| A4 | 3 | Linear（直线，实为螺旋） | dLowerA4 / dUpperA4 |
注意：A4轴虽然物理上是旋转螺旋关节，但此处声明为 Linear 类型，这是 SoftMotion ZC耦合运动学的特殊处理方式，确保控制器正确处理其线性等效输入。
AxesToCartesian
输入输出(In_out)(const): a (TRAFO.AXISPOS_REF) — 输入：各轴位置（度/mm） 输入输出(In_Out): f (TRAFO.SMC_Frame) — 输出：TCP的位姿（位置+姿态矩阵） cd (TRAFO.CONFIGDATA) — 输出：序列化的构型数据 返回值类型: TRAFO.SMC_Error
功能：正运动学（Forward Kinematics, FK）。从关节空间坐标计算TCP在笛卡尔空间中的位姿。
说明：AxesToCartesian 应接收关节空间位置并输出笛卡尔位姿及构型数据；输出位姿需与 CONFIGDATA 保持一致以支持逆解与路径规划；发生几何或矩阵计算异常时应返回标准错误码。返回 SMC_NO_ERROR — 计算成功 - SMC_CP_ERROR_CREATING_PARAM_TRANSFORM — 旋转矩阵创建失败。
CartesianToAxes
输入输出(In_Out): a (TRAFO.AXISPOS_REF) — 输出：各轴位置（度/mm） 输入输出(In_out)(const): f (TRAFO.SMC_Frame) — 输入：TCP的位姿 aRef (TRAFO.AXISPOS_REF) — 输入：参考轴位置（用于消解歧义） cd (TRAFO.CONFIGDATA) — 输入：构型数据（肘部选择/周期选择） 返回值类型: TRAFO.SMC_Error
功能：逆运动学（Inverse Kinematics, IK）。从TCP的笛卡尔位姿计算各关节轴位置。
说明： 1. 输入笛卡尔位姿、构型数据与参考轴位置，输出关节空间位置； 2. 必须支持构型约束与周期消歧，避免解跳变； 3. 目标超出可达域时返回标准位置超限错误码。以SCARA模型为例：该子类可完成逆解、耦合补偿与周期消歧。aRef 用于解决旋转轴多圈（>360°范围）时的歧义，使结果尽可能接近当前实际位置，避免轴跳变。返回值SMC_NO_ERROR — 计算成功 - SMC_AXIS_INVERSE_TRAFO_EXCEEDING_POSLIMITS — 目标位置超出工作空间
AxesToOrientation
输入输出(In_out)(const): a (TRAFO.AXISPOS_REF) — 输入：各轴位置 输入输出(In_out): m (TRAFO.SMC_Matrix3) — 输出：法兰坐标系的姿态旋转矩阵 返回值类型: TRAFO.SMC_ERROR
功能：从关节轴位置计算法兰坐标系的姿态（旋转矩阵），是 AxesToCartesian 的姿态子集提取。
说明：可通过复用正解结果提取姿态矩阵，或独立实现姿态求解；输出需与 AxesToCartesian 保持一致。
IsSingularity
输入输出(In_out)(const): a (TRAFO.AXISPOS_REF) — 输入：各轴位置 返回值: BOOL
功能：判断给定关节位置是否处于奇异点。
说明：需要给出可复现的奇异判定规则，并在奇异附近保证返回值稳定一致。返回值TRUE — 处于奇异位置；FALSE — 非奇异。
GetOrientationImage
输入输出(In_out): oriInfo (TRAFO.OrientationSpace) — 输出：法兰坐标系的姿态空间描述
功能：向 SoftMotion 描述该运动学变换所能产生的所有法兰坐标系姿态信息。用于 SoftMotion 内部的姿态插补规划。
说明：明确姿态自由度、零位姿态和主旋转轴，以支持上层插补器进行姿态空间规划。
以SCARA模型为例说明： - oriInfo.dof = 0：法兰姿态自由度为0（仅可绕Z轴旋转，无独立姿态自由度） - oriInfo.mOriZero = 单位矩阵：零位时法兰坐标系与MCS对齐 - oriInfo.vRot = (0, 0, 1)：旋转轴为Z轴 - oriInfo.vU = (0, 0, 1)：法兰Z轴方向与MCS的Z轴对齐。
GetConfigurationDataSize
返回值类型: INT
功能：返回该运动学模型所使用的构型数据（CONFIGDATA）的字节大小，为常量值。以Scara机器人为例，固定返回 2，对应ConfigData.abyData[0]：肘部选择（0=左肘, 1=右肘)。
GetDefaultConfigurationData
输入输出(In_out): cd (TRAFO.CONFIGDATA) — 输出：默认构型数据 返回值: BOOL
功能：返回运动学的默认构型配置，序列化到 CONFIGDATA 结构中，供初始化时使用。返回值：TRUE — 成功返回默认构型
CPConnectible
输入输出(In_out)(const): cd1 (TRAFO.CONFIGDATA) — 输入：构型1 输入输出(In_out)(const): cd2 (TRAFO.CONFIGDATA) — 输入：构型2 返回值: BOOL
功能：判断两个构型之间是否可以进行连续路径（CP）插补，而不会导致轴位置跳变。SoftMotion 用此方法判断相邻路径段是否可以平滑衔接。
接口特性（满足等价关系）： - 自反性：CPConnectible(c, c) = TRUE - 对称性：CPConnectible(c1, c2) = CPConnectible(c2, c1) - 传递性：若 c1↔︎c2 且 c2↔︎c3，则 c1↔︎c3
AxesToConfiguration_Offset
输入输出(In_out)(const): a (TRAFO.AXISPOS_REF) — 输入：各轴位置 输入输出(In_out)(const): vOffset_TCP (TRAFO.SMC_Vector3D) — 输入：相对TCP坐标系的位置偏移量 输入输出(In_out): cd (TRAFO.CONFIGDATA) — 输出：偏移运动学的构型数据
功能：在考虑额外工具偏移量的情况下，从关节位置计算偏移TCP对应的构型数据。属于 ISMPositionKinematics_Offset2 接口的一部分，用于工具偏移补偿场景。
说明：当 vOffset_TCP 为零向量时，返回结果与 AxesToCartesian 的 cd 输出相同。当偏移不为零时，等效修改了连杆几何（如同加长了第二连杆），可能导致与原始运动学不同的肘部构型。
CartesianToAxes_Offset
输入输出(In_out): a (TRAFO.AXISPOS_REF) — 输出：各轴位置 输入输出(In_out)(const): v_MCS (TRAFO.SMC_Vector3D) — 输入：偏移TCP在MCS中的位置 vOffset_TCP (TRAFO.SMC_Vector3D) — 输入：TCP坐标系下的工具偏移量 aRef (TRAFO.AXISPOS_REF) — 输入：参考轴位置 cd (TRAFO.CONFIGDATA) — 输入：偏移运动学的构型数据 返回值类型: TRAFO.SMC_Error
功能：在已知工具偏移量的情况下，从偏移TCP的笛卡尔位置计算关节轴位置。属于 ISMPositionKinematics_Offset2 接口，用于处理工具中心点（TCP）不在法兰原点的情况。
说明：由于法兰姿态在调用时未知，标准 CartesianToAxes 无法直接处理工具偏移；此方法通过位置运动学的几何特性来处理偏移。当 vOffset_TCP 为零时，结果等同于直接调用 CartesianToAxes。
补充说明：以SCARA模型为例CONFIGDATA.abyData 各字节含义：
| 字节索引 | 变量 | 含义 | 取值说明 |
| abyData[0] | 肘部选择 | 控制IK肘部朝向 | 2=左肘（A20），0=自动跟随当前状态 |
| abyData[1] | nPeriod4 | A4轴旋转周期选择 | 0=自动选最近周期；0=选取周期 [-180°+360°*(n-1), 180°+360°*(n-1)] |
通过 SMC_SetKinConfiguration 功能块可在运动过程中动态修改构型数据。
支持构型
| 类型 | 备注 | 简图（有代表性的） |
| Scara类 | 3轴和4轴：Kin_Scara3_ZCCC、Kin_Scara3_CCZ、Kin_Scara4_CCZC_ZCcoupled、Kin_Scara4_ZCcoupled_Type1 | |
| Delta类 | Kin_Delta_P3C_Type3 | |
| Tripod类 | Kin_Tripod_P3Z | |
| 并联2D类 | Kin_P2C_Type1、Kin_P2C_Type2、Kin_P2Y、Kin_PCC、Kin_P2X_Type1、Kin_P2X_Type2 | |
| 串联2D类 | Kin_SCCC | |
| 通用6轴类 | Kin_Ac6_RZRYRYRXRYRZ_J6Offset、Kin_Ac6_TXTYTZRYRXRZ | |
Scara类构型
Scara类构型为“平面2转+竖直1直线+可能的额外末端轴”（常见3轴/4轴变体），在笛卡尔空间里容易形成“旋转平面主导、Z向独立”的几何结构。
工作空间在XY平面通常近似环形/扇形；Z轴一般为线性独立运动。标准图例如下：
Delta类构型
Delta类为并联结构（多根伸缩杆/连杆），关节到末端的映射是并联约束，动力学和刚度通常更有优势。工作空间通常是“甜甜圈/倒金字塔”形（顶部更大、中部/边缘逐渐收敛），对速度与加速度友好。标准图例如下：
通用6轴类构型
典型是6个旋转关节，形成较完整的姿态可达能力（6自由度齐全），笛卡尔位置+姿态通常都可直接描述。逆运动学通常存在多解（常见“腕部/肘部”分支），需要构型数据来消解歧义并保证连续性。奇异性通常以“腕部奇异/肘部奇异”等形式出现。标准图例如下：
Tripod类构型
典型为并联“3支臂/3约束”的构型（常用于实现特定姿态空间的并联约束），其末端位姿可达域通常由并联杆长与几何约束决定。标准图例如下：
并联平面类构型
结构特征：末端（或执行点）由多条分支/连杆同时约束到同一平面，通常是“多约束并行几何映射”。工作空间：常呈环形/甜甜圈或倒V等由并联约束决定的形状，越接近边界越敏感。标准图例如下：
串联平面类构型
结构特征：执行点由按顺序连接的关节链在平面内完成映射，典型是 2转/1转+1伸/等效平面2自由度链。工作空间通常更规则（椭圆/环形带），边界由连杆长度与角度可达性决定。图例如下：