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.

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

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). 🚦

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 also included.

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);

🖥️ 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 📚

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