File syn_kinematics.c¶

FileList > motor > syn_kinematics.c

Multi-Axis Robot Forward & Inverse Kinematics Engine Implementation.

#include "../port/syn_port_system.h"
#include "syn_kinematics.h"
#include <string.h>

Public Functions¶

Type	Name
SYN_Status	syn_dh_matrix (const SYN_DH_Param * param, q16_t joint_val, SYN_DH_Convention conv, SYN_Matrix * out_t44) Compute single-link 4x4 Homogeneous Transformation Matrix from DH parameters.
SYN_Status	syn_kinematics_6dof_fk (const SYN_Kinematics_6DOFConfig * cfg, const q16_t q, SYN_Pose6D * out_pose) Forward kinematics for 6-DOF articulated arm.
SYN_Status	syn_kinematics_6dof_ik (const SYN_Kinematics_6DOFConfig * cfg, const SYN_Pose6D * target, SYN_ArmElbow elbow, bool wrist_flip, q16_t out_q) Closed-form inverse kinematics for 6-DOF arm with spherical wrist (Pieper solution).
SYN_Status	syn_kinematics_delta_fk (const SYN_Kinematics_DeltaConfig * cfg, q16_t theta1, q16_t theta2, q16_t theta3, SYN_Position3D * out_pos) Forward kinematics for 3-axis Delta parallel robot.
SYN_Status	syn_kinematics_delta_ik (const SYN_Kinematics_DeltaConfig * cfg, const SYN_Position3D * target, q16_t * out_theta1, q16_t * out_theta2, q16_t * out_theta3) Inverse kinematics for 3-axis Delta parallel robot.
SYN_Status	syn_kinematics_forward (const SYN_DH_Param * chain, size_t num_joints, const q16_t * joint_vals, SYN_DH_Convention conv, SYN_Matrix * out_t44, SYN_Pose6D * out_pose) Compute Forward Kinematics for a general serial DH chain.
SYN_Status	syn_kinematics_jacobian (const SYN_DH_Param * chain, size_t num_joints, const q16_t * joint_vals, SYN_DH_Convention conv, SYN_Matrix * out_j6xn) Calculate Geometric 6xN Jacobian Matrix for a serial link chain.
SYN_Status	syn_kinematics_planar3_fk (const SYN_Kinematics_Planar3Config * cfg, q16_t q1, q16_t q2, q16_t q3, q16_t * out_x, q16_t * out_y, q16_t * out_phi) Forward kinematics for 3-DOF planar arm.
SYN_Status	syn_kinematics_planar3_ik (const SYN_Kinematics_Planar3Config * cfg, q16_t x, q16_t y, q16_t phi, SYN_ArmElbow elbow, q16_t * out_q1, q16_t * out_q2, q16_t * out_q3) Closed-form inverse kinematics for 3-DOF planar arm.
SYN_Status	syn_kinematics_scara_fk (const SYN_Kinematics_SCARAConfig * cfg, q16_t q1, q16_t q2, q16_t d3, q16_t q4, SYN_Pose6D * out_pose) Forward kinematics for 4-DOF SCARA robot.
SYN_Status	syn_kinematics_scara_ik (const SYN_Kinematics_SCARAConfig * cfg, const SYN_Pose6D * target, SYN_ArmElbow elbow, q16_t * out_q1, q16_t * out_q2, q16_t * out_d3, q16_t * out_q4) Closed-form inverse kinematics for 4-DOF SCARA robot.

Public Static Functions¶

Type	Name
SYN_Status	delta_calc_arm (q16_t x0, q16_t y0, q16_t z0, q16_t L, q16_t l, q16_t * out_theta) Helper to solve single Delta arm angle.
void	rot_matrix_to_rpy (const q16_t r33, SYN_Orientation3D * out_ori) Extract Roll-Pitch-Yaw Euler angles from 3x3 rotation matrix.
void	rpy_to_rot_matrix (q16_t roll, q16_t pitch, q16_t yaw, q16_t r33) Build 3x3 rotation matrix from Roll-Pitch-Yaw Euler angles (Z-Y-X order).

Macros¶

Type	Name
define	Q16_COS120 `(([q16\_t](syn__qmath_8h.md#typedef-q16_t)) - 32768)`
define	Q16_COS240 `(([q16\_t](syn__qmath_8h.md#typedef-q16_t)) - 32768)`
define	Q16_SIN120 `56756`
define	Q16_SIN240 `(([q16\_t](syn__qmath_8h.md#typedef-q16_t)) - 56756)`
define	Q16_SQRT3_2 `56756`

Public Functions Documentation¶

function syn_dh_matrix¶

Compute single-link 4x4 Homogeneous Transformation Matrix from DH parameters.

SYN_Status syn_dh_matrix (
    const SYN_DH_Param * param,
    q16_t joint_val,
    SYN_DH_Convention conv,
    SYN_Matrix * out_t44
)

Parameters:

param DH parameters for link.
joint_val Joint variable (angle in rad for revolute, displacement for prismatic).
conv DH convention (standard or modified).
out_t44 Output 4x4 matrix (must be initialized with 4 rows, 4 cols).

Returns:

SYN_OK on success, SYN_INVALID_PARAM on NULL or dimension error.

function syn_kinematics_6dof_fk¶

Forward kinematics for 6-DOF articulated arm.

SYN_Status syn_kinematics_6dof_fk (
    const SYN_Kinematics_6DOFConfig * cfg,
    const q16_t q,
    SYN_Pose6D * out_pose
)

Parameters:

cfg 6-DOF arm configuration.
q Array of 6 joint angles in radians.
out_pose Output 6-DOF Cartesian pose.

Returns:

SYN_OK on success.

function syn_kinematics_6dof_ik¶

Closed-form inverse kinematics for 6-DOF arm with spherical wrist (Pieper solution).

SYN_Status syn_kinematics_6dof_ik (
    const SYN_Kinematics_6DOFConfig * cfg,
    const SYN_Pose6D * target,
    SYN_ArmElbow elbow,
    bool wrist_flip,
    q16_t out_q
)

Parameters:

cfg 6-DOF arm configuration.
target Target 6-DOF pose.
elbow Shoulder/elbow solution choice.
wrist_flip Invert wrist pitch/yaw solution if true.
out_q Output array of 6 joint angles in radians.

Returns:

SYN_OK on success, SYN_ERROR if target position or orientation unreachable.

function syn_kinematics_delta_fk¶

Forward kinematics for 3-axis Delta parallel robot.

SYN_Status syn_kinematics_delta_fk (
    const SYN_Kinematics_DeltaConfig * cfg,
    q16_t theta1,
    q16_t theta2,
    q16_t theta3,
    SYN_Position3D * out_pos
)

Parameters:

cfg Delta robot geometry configuration.
theta1 Actuator 1 angle (rad).
theta2 Actuator 2 angle (rad).
theta3 Actuator 3 angle (rad).
out_pos Output 3D Cartesian position of traveling plate center.

Returns:

SYN_OK on success, SYN_ERROR if no real geometric intersection exists.

function syn_kinematics_delta_ik¶

Inverse kinematics for 3-axis Delta parallel robot.

SYN_Status syn_kinematics_delta_ik (
    const SYN_Kinematics_DeltaConfig * cfg,
    const SYN_Position3D * target,
    q16_t * out_theta1,
    q16_t * out_theta2,
    q16_t * out_theta3
)

Parameters:

cfg Delta robot geometry configuration.
target Target 3D Cartesian position (X, Y, Z).
out_theta1 Output actuator 1 angle (rad).
out_theta2 Output actuator 2 angle (rad).
out_theta3 Output actuator 3 angle (rad).

Returns:

SYN_OK on success, SYN_ERROR if position is outside workspace.

function syn_kinematics_forward¶

Compute Forward Kinematics for a general serial DH chain.

SYN_Status syn_kinematics_forward (
    const SYN_DH_Param * chain,
    size_t num_joints,
    const q16_t * joint_vals,
    SYN_DH_Convention conv,
    SYN_Matrix * out_t44,
    SYN_Pose6D * out_pose
)

Parameters:

chain Array of DH joint parameters.
num_joints Number of joints in chain (<= SYN_KINEMATICS_MAX_JOINTS).
joint_vals Array of joint values.
conv DH convention.
out_t44 Optional output 4x4 end-effector transform matrix (can be NULL).
out_pose Optional output 6-DOF pose (can be NULL).

Returns:

SYN_OK on success, SYN_INVALID_PARAM on invalid inputs.

function syn_kinematics_jacobian¶

Calculate Geometric 6xN Jacobian Matrix for a serial link chain.

SYN_Status syn_kinematics_jacobian (
    const SYN_DH_Param * chain,
    size_t num_joints,
    const q16_t * joint_vals,
    SYN_DH_Convention conv,
    SYN_Matrix * out_j6xn
)

Parameters:

chain Array of DH joint parameters.
num_joints Number of joints in chain.
joint_vals Current joint positions.
conv DH convention.
out_j6xn Output 6xN matrix (rows=6, cols=num_joints).

Returns:

SYN_OK on success, SYN_INVALID_PARAM on invalid inputs.

function syn_kinematics_planar3_fk¶

Forward kinematics for 3-DOF planar arm.

SYN_Status syn_kinematics_planar3_fk (
    const SYN_Kinematics_Planar3Config * cfg,
    q16_t q1,
    q16_t q2,
    q16_t q3,
    q16_t * out_x,
    q16_t * out_y,
    q16_t * out_phi
)

Parameters:

cfg Planar arm configuration.
q1 Joint 1 angle (rad).
q2 Joint 2 angle (rad).
q3 Joint 3 angle (rad).
out_x Pointer to receive X position.
out_y Pointer to receive Y position.
out_phi Pointer to receive tool orientation angle (rad).

Returns:

SYN_OK on success.

function syn_kinematics_planar3_ik¶

Closed-form inverse kinematics for 3-DOF planar arm.

SYN_Status syn_kinematics_planar3_ik (
    const SYN_Kinematics_Planar3Config * cfg,
    q16_t x,
    q16_t y,
    q16_t phi,
    SYN_ArmElbow elbow,
    q16_t * out_q1,
    q16_t * out_q2,
    q16_t * out_q3
)

Parameters:

cfg Planar arm configuration.
x Target X position.
y Target Y position.
phi Target end-effector orientation angle (rad).
elbow Elbow configuration (up or down).
out_q1 Output joint 1 angle.
out_q2 Output joint 2 angle.
out_q3 Output joint 3 angle.

Returns:

SYN_OK on success, SYN_ERROR if target unreachable.

function syn_kinematics_scara_fk¶

Forward kinematics for 4-DOF SCARA robot.

SYN_Status syn_kinematics_scara_fk (
    const SYN_Kinematics_SCARAConfig * cfg,
    q16_t q1,
    q16_t q2,
    q16_t d3,
    q16_t q4,
    SYN_Pose6D * out_pose
)

Parameters:

cfg SCARA configuration.
q1 Joint 1 shoulder angle (rad).
q2 Joint 2 elbow angle (rad).
d3 Joint 3 prismatic vertical displacement (distance down).
q4 Joint 4 wrist roll angle (rad).
out_pose Output 4-DOF pose (X, Y, Z, Yaw).

Returns:

SYN_OK on success.

function syn_kinematics_scara_ik¶

Closed-form inverse kinematics for 4-DOF SCARA robot.

SYN_Status syn_kinematics_scara_ik (
    const SYN_Kinematics_SCARAConfig * cfg,
    const SYN_Pose6D * target,
    SYN_ArmElbow elbow,
    q16_t * out_q1,
    q16_t * out_q2,
    q16_t * out_d3,
    q16_t * out_q4
)

Parameters:

cfg SCARA configuration.
target Target 6-DOF pose (X, Y, Z, Yaw).
elbow Elbow configuration (up/left or down/right).
out_q1 Output joint 1 angle.
out_q2 Output joint 2 angle.
out_d3 Output joint 3 vertical position.
out_q4 Output joint 4 wrist angle.

Returns:

SYN_OK on success, SYN_ERROR if target unreachable.

Public Static Functions Documentation¶

function delta_calc_arm¶

Helper to solve single Delta arm angle.

static SYN_Status delta_calc_arm (
    q16_t x0,
    q16_t y0,
    q16_t z0,
    q16_t L,
    q16_t l,
    q16_t * out_theta
)

Parameters:

x0 Projected X coordinate.
y0 Projected Y coordinate.
z0 Projected Z coordinate.
L Upper arm length.
l Lower arm parallelogram length.
out_theta Pointer to receive solved arm angle.

Returns:

SYN_OK on success, SYN_ERROR if target is unreachable.

function rot_matrix_to_rpy¶

Extract Roll-Pitch-Yaw Euler angles from 3x3 rotation matrix.

static void rot_matrix_to_rpy (
    const q16_t r33,
    SYN_Orientation3D * out_ori
)

Parameters:

r33 Flat 9-element Q16 array.
out_ori Output 3D orientation.

function rpy_to_rot_matrix¶

Build 3x3 rotation matrix from Roll-Pitch-Yaw Euler angles (Z-Y-X order).

static void rpy_to_rot_matrix (
    q16_t roll,
    q16_t pitch,
    q16_t yaw,
    q16_t r33
)

Parameters:

roll Roll angle (rad).
pitch Pitch angle (rad).
yaw Yaw angle (rad).
r33 Flat 9-element Q16 array for 3x3 rotation matrix.

Macro Definition Documentation¶

define Q16_COS120¶

#define Q16_COS120 `(( q16_t ) - 32768)`

cos(120 deg) in Q16

define Q16_COS240¶

#define Q16_COS240 `(( q16_t ) - 32768)`

cos(240 deg) in Q16

define Q16_SIN120¶

#define Q16_SIN120 `56756`

sin(120 deg) in Q16

define Q16_SIN240¶

#define Q16_SIN240 `(( q16_t ) - 56756)`

sin(240 deg) in Q16

define Q16_SQRT3_2¶

#define Q16_SQRT3_2 `56756`

sqrt(3)/2 ≈ 0.866025 in Q16

The documentation for this class was generated from the following file src/syntropic/motor/syn_kinematics.c