## 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/](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):**

| Joint | a [m]  | d [m]  | α [rad] | θ [rad] |
| ----: | :----: | :----: | :-----: | :-----: |
|    J1 |   0    | **D1** |  +π/2   | **θ1**  |
|    J2 | **A2** |   0    |    0    | **θ2**  |
|    J3 | **A3** |   0    |    0    | **θ3**  |
|    J4 |   0    | **D4** |  +π/2   | **θ4**  |
|    J5 |   0    | **D5** |  −π/2   | **θ5**  |
|    J6 |   0    | **D6** |    0    | **θ6**  |

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

**Members of Common.IUrDhParameters**
```csharp
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 🧩

```csharp
using UnderAutomation.UniversalRobots.Kinematics;

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

### Custom DH parameters 🔧

```csharp
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.

```csharp
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.

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

**Members of Kinematics.KinematicsResult**
```csharp
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; }
}
```

**Members of Kinematics.TransformationSet**
```csharp
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 also included.

```csharp
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.

```csharp
using UnderAutomation.UniversalRobots.Kinematics;

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

**Members of Kinematics.SingularityType**
```csharp
[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](/universal-robots/download) and give it a spin!

![](/universal-robots/WinformsScreenshots/Kinematics.jpg)

## API Reference 📚

**Members of Kinematics.KinematicsUtils**
```csharp
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](https://ieeexplore.ieee.org/document/7988522)