Forward and Inverse Kinematics

What is Kinematics? 🤖✨

The tools in the UnderAutomation.UniversalRobots.Kinematics namespace let you compute forward and inverse kinematics for Universal Robots cobots—without connecting to a robot.

In practice, that means you can:

• Convert joint angles → cartesian tool pose (position + orientation) via forward kinematics, and
• Convert cartesian tool pose → joint angles (often multiple solutions) via inverse kinematics.

DH Parameters 🧱

Denavit–Hartenberg (DH) parameters are a standardized way to describe a robot arm’s geometry. Each joint uses four parameters:

• θ (theta): Joint angle (rotation around the z-axis)
• d: Link offset (distance along the z-axis)
• a: Link length (distance along the x-axis)
• α (alpha): Link twist (rotation around the x-axis)

For more background on DH parameters, see Universal Robots’ reference: https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/

DH parameters are essential for modeling a robot. For UR cobots, only A2, A3, D1, D4, D5, D6 vary across models.

Corrected DH table (θ are the joint variables):

The IUrDhParameters interface lets you define DH parameters for a specific robot model.

public interface IUrDhParameters {
 
    // DH parameter a2 (Shoulder)
    double A2 { get; }
 
    // DH parameter a3 (Elbow)
    double A3 { get; }
 
    // DH parameter d1 (Base)
    double D1 { get; }
 
    // DH parameter d4 (Wrist1)
    double D4 { get; }
 
    // DH parameter d5 (Wrist2)
    double D5 { get; }
 
    // DH parameter d6 (Wrist3/Tool)
    double D6 { get; }
}

Default DH parameters from a model 🧩

using UnderAutomation.UniversalRobots.Kinematics;

// Get default DH parameters for the UR3e model
IUrDhParameters dhParameters = KinematicsUtils.GetDhParametersFromModel(RobotModelsExtended.UR3e);

Custom DH parameters 🔧

using UnderAutomation.UniversalRobots.Kinematics;

// Define custom DH parameters
IUrDhParameters dhParameters = new CustomUrDhParameters(
    a2: 0.165,  // Link length for joint 2
    a3: 0.135,  // Link length for joint 3
    d1: 0.1519, // Link offset for joint 1
    d4: 0.11235,// Link offset for joint 4
    d5: 0.08535,// Link offset for joint 5
    d6: 0.0819  // Link offset for joint 6
);

DH parameters and pose from a connected robot 🔌

Via the Primary Interface, you can read the connected robot’s DH parameters. The ConfigurationData and KinematicsInfo packets both expose DH parameters.

using UnderAutomation.UniversalRobots;
using UnderAutomation.UniversalRobots.PrimaryInterface;

var robot = new UR();

var param = new ConnectParameters("192.168.0.1");
param.PrimaryInterface.Enable = true; // enable Primary Interface
robot.Connect(param);

// ... wait for packet to be received

// DH parameters from the connected robot (both properties carry the same DH info)
IUrDhParameters dhParametersFromConfigurationData = robot.PrimaryInterface.ConfigurationData;
IUrDhParameters dhParametersFromKinematicsInfo = robot.PrimaryInterface.KinematicsInfo;

// Current joint positions in radians
var jointData = robot.PrimaryInterface.JointData;
double[] jointPositions = new double[] {
    jointData.Base.Position,
    jointData.Shoulder.Position,
    jointData.Elbow.Position,
    jointData.Wrist1.Position,
    jointData.Wrist2.Position,
    jointData.Wrist3.Position,
};

// Current cartesian pose as Pose (X, Y, Z, RX, RY, RZ)
Pose currentPose = robot.PrimaryInterface.CartesianInfo.AsPose();

Forward Kinematics 🚀

Forward kinematics computes the end-effector (tool) position and orientation from known joint angles.

using UnderAutomation.UniversalRobots.Kinematics;

// Default DH parameters for UR3e
IUrDhParameters dhParameters = KinematicsUtils.GetDhParametersFromModel(RobotModelsExtended.UR3e);

// Forward kinematics for given joint angles (radians)
KinematicsResult fkResult = KinematicsUtils.ForwardKinematics(new double[] { 0, -1.57, 1.57, 0, 0, 0 }, dhParameters);

// Convert the 4x4 transform matrix to a Pose (X, Y, Z, RX, RY, RZ)
Pose cartesianPosition = Pose.From4x4MatrixToRotationVector(fkResult.ToolTransform);

KinematicsResult contains the tool’s cartesian transform (ToolTransform, 4×4). It also exposes TransformationSet members, IndividualLocalTransforms and CumulativeGlobalTransforms, which provide—for each joint—the local transform and the cumulative transform (this joint composed with all previous ones). 🚦

public class KinematicsResult {
     public KinematicsResult()
 
    // Cumulative local transformation matrices of each joint
    public TransformationSet CumulativeGlobalTransforms { get; set; }
 
    // Individual local transformation matrices of each joint
    public TransformationSet IndividualLocalTransforms { get; set; }
 
    // 4x4 transformation matrix of the tool
    public double[,] ToolTransform { get; set; }
}

public class TransformationSet {
     public TransformationSet()
 
    // 4x4 transformation matrix of the base joint 1
    public double[,] Base { get; set; }
 
    // 4x4 transformation matrix of the elbow joint 3
    public double[,] Elbow { get; set; }
 
    // 4x4 transformation matrix of the shoulder joint 2
    public double[,] Shoulder { get; set; }
 
    // 4x4 transformation matrix of the wrist1 joint 4
    public double[,] Wrist1 { get; set; }
 
    // 4x4 transformation matrix of the wrist2 joint 5
    public double[,] Wrist2 { get; set; }
 
    // 4x4 transformation matrix of the wrist3 (Tool) joint 6
    public double[,] Wrist3 { get; set; }
}

Inverse Kinematics 🧭

Inverse kinematics finds joint angles that achieve a requested tool pose (position + orientation). Use GetNearestSolution to pick the solution closest to a reference joint configuration. Solutions that encounter singularities are discarded automatically.

using UnderAutomation.UniversalRobots.Kinematics;

// Default DH parameters for UR3e
IUrDhParameters dhParameters = KinematicsUtils.GetDhParametersFromModel(RobotModelsExtended.UR3e);

var pose = new Pose(0.3, 0.2, 0.5, 0, 3.14, 0);

// Compute IK for the given cartesian pose
var matrixTransform = pose.FromRotationVectorTo4x4Matrix();
double[][] ikSolutions = KinematicsUtils.InverseKinematics(matrixTransform, dhParameters);

// Each solution is an array of 6 joint angles (radians)
foreach (var solution in ikSolutions)
{
    Console.WriteLine("Solution:");
    for (int i = 0; i < 6; i++)
        Console.WriteLine($"Joint {i + 1}: {solution[i]} rad");
}

Detecting Singularities ⚠️

A kinematic singularity occurs when the robot effectively loses a degree of freedom, which can cause unpredictable motion or unreachable poses. In such configurations, IK may be ambiguous or unsolvable.

Use GetSingularity to check whether a given joint configuration is close to a singularity.

using UnderAutomation.UniversalRobots.Kinematics;

// Get singularity flags for given joint angles and DH parameters
SingularityType singularity = KinematicsUtils.GetSingularity(elbow, shoulder, wrist1, wrist2, dhParameters);

[Flags]
public enum SingularityType {
 
    // Elbow singularity
    Elbow = 2
 
    // No singularity
    None = 0
 
    // Shoulder singularity
    Shoulder = 4
 
    // Wrist singularity
    Wrist = 1
}

🖥️ Try It Out with the Windows Example!

Want to see this in action? A special WinForms page lets you test kinematics calculations live! 🎮

👉 Download it here and give it a spin!

API Reference 📚

public static class KinematicsUtils {
 
    // Denavit–Hartenberg homogeneous transform
    public static double[,] DHTransform(double theta, double d, double a, double alpha)
 
    // Forward kinematics : compute tool transform and intermediate transforms from joint angles (radians) and DH parameters.
    public static KinematicsResult ForwardKinematics(double[] jointAnglesRad, IUrDhParameters dhParameters)
 
    // Get the DH parameters for a given robot model.
    public static IUrDhParameters GetDhParametersFromModel(RobotModelsExtended model)
 
    // Pick the solution nearest to a reference joint vector (L1 distance). Null if invalid inputs.
    public static double[] GetNearestSolution(double[][] jointSolutions, double[] jointReference)
 
    // Singularity detection using det(J) factors: s5≈0 (wrist), s3≈0 (elbow),
    // and c2*a2 + c23*a3 + s234*d5≈0 (shoulder).
    public static SingularityType GetSingularity(double elbow, double shoulder, double wrist1, double wrist2, IUrDhParameters dhParameters)
 
    // Homogeneous matrix multiplication optimized for DH transforms.
    public static double[,] HomogeneousMultiply(double[,] A, double[,] B)
 
    // Analytical inverse kinematics
    // Returns a list of candidate joint vectors; filters out singularities.
    public static double[][] InverseKinematics(double[,] toolTransform, IUrDhParameters dhParameters)
}

Implementation notes 🧪

The implementation adopts a closed-form approach for a 6-DOF serial cobot, adhering to the standard DH convention and the analytical sequence reported by Chen et al., IEEE ICASI 2017. Forward kinematics follows the homogeneous transform in Eq. (1.1) and the chain product T₀⁶ = ∏ Tᵢ₋₁ᵢ in Eq. (1.2); inverse kinematics resolves joint variables in the classical order—q₁ (Eq. (1.12)), q₅ (Eq. (1.15)), q₆ (Eq. (1.17)), the compound angle q₂₃₄ (Eq. (1.20)), then q₂ (Eq. (1.25)), with q₃ and q₄ recovered from (Eq. (1.27)). Singularity proximity is assessed through a Jacobian determinant factorization consistent with the paper, namely det(J) ∝ s₃·s₅·a₂·a₃·(c₂·a₂ + c₂₃·a₃ + s₂₃₄·d₅), ensuring pathological configurations are identified and filtered during solution assembly. In addition to strict numerical conditioning (branching on trigonometric degeneracies, graceful handling when S₅ → 0, and robust quadrant resolution), the algorithm is optimized for performance and robustness through invariant precomputation, minimal heap allocation, and fast-path evaluation for common pose families—yielding real-time behavior without sacrificing accuracy.

see : Chen et al., IEEE ICASI 2017