Inverse kinematics (IK) and forward kinematics (FK) let you move between joint space and Cartesian space. FK computes the tool p
ose from known joint angles, while IK finds joint angles for a desired pose. The kinematics utilities in the Fanuc SDK are **off
line helpers**: you can evaluate poses and joint solutions without connecting to a controller, making them perfect for simulatio
n, path validation, and pre-deployment checks.

## Why this matters for both industrial arms and cobots

The SDK ships with two analytical solvers and automatically chooses the right one for you:

- **OPW industrial arms**: Classical 6-axis Fanuc robots with an ortho-parallel base and spherical wrist, based on the paper `An Analytical Solution of the Inverse Kinematics Problem of Industrial Serial Manipulators with an Ortho-parallel Basis and a Spherical Wrist` by Mathias Brandstötter, Arthur Angerer, and Michael Hofbaur.
- **CRX collaborative arms**: Fanuc CRX cobots that have their own closed-form solver and optional dual solutions, based on paper `Geometric Approach for Inverse Kinematics of the FANUC CRX Collaborative Robot` by Manel Abbes and Gérard Poisson.

When you call `KinematicsUtils.InverseKinematics()`, it inspects `DhParameters.KinematicsCategory`:

- `KinematicsCategory.Opw` 👉 dispatches to `Opw.OpwKinematicsUtils.InverseKinematics` (industrial robots).
- `KinematicsCategory.Crx` 👉 dispatches to `Crx.CrxKinematicsUtils.InverseKinematics` (CRX cobots).

That means you can rely on a single entry point and let the SDK route the request appropriately. 🚦

## Typical workflows

### Run forward kinematics (FK)

**C# : KinematicsFK**
```csharp
using UnderAutomation.Fanuc.Common;
using UnderAutomation.Fanuc.Kinematics;

public class KinematicsFK
{
    static void Main()
    {
        /**/
        // Load robot geometry
        var dh = DhParameters.FromArmKinematicModel(ArmKinematicModels.CRX10iA);

        // Joint angles in degrees (Fanuc convention)
        var jointsDeg = new JointsPosition { J1 = 0, J2 = -30, J3 = 45, J4 = 0, J5 = 60, J6 = 90 };

        // Compute pose: returns XYZ + WPR
        CartesianPosition pose = KinematicsUtils.ForwardKinematics(jointsDeg, dh);
        /**/
    }
}
```

**Python : KinematicsFK**
```python
from underautomation.fanuc.kinematics.kinematics_utils import KinematicsUtils
from underautomation.fanuc.kinematics.dh_parameters import DhParameters
from underautomation.fanuc.common.joints_position import JointsPosition

##
# Load robot geometry
dh = DhParameters.from_arm_kinematic_model("CRX10iA")

# Joint angles in degrees (Fanuc convention)
joints_deg = JointsPosition(j1=0, j2=-30, j3=45, j4=0, j5=60, j6=90)

# Compute pose: returns XYZ + WPR
pose = KinematicsUtils.forward_kinematics(joints_deg, dh)
##
```

### Solve inverse kinematics (IK) for an OPW robot

**C# : KinematicsIK**
```csharp
using UnderAutomation.Fanuc.Common;
using UnderAutomation.Fanuc.Kinematics;
using UnderAutomation.Fanuc.Kinematics.Crx;

public class KinematicsIK
{
    static void Main()
    {
        /**/
        // OPW industrial robot
        var dh = DhParameters.FromArmKinematicModel(ArmKinematicModels.ARCMate120iD);
        var target = new CartesianPosition { X = 800, Y = 0, Z = 450, W = 180, P = 0, R = 90 };
        JointsPosition[] solutions = KinematicsUtils.InverseKinematics(target, dh);

        // CRX cobot with dual solutions
        var dhCrx = DhParameters.FromArmKinematicModel(ArmKinematicModels.CRX10iAL);
        var targetCrx = new CartesianPosition { X = 400, Y = 250, Z = 650, W = 0, P = 90, R = 0 };
        JointsPosition[] crxSolutions = CrxKinematicsUtils.InverseKinematics(
            targetCrx, dhCrx,
            includeDuals: true,
            seedJoints: new double[] { 0, -20, 40, 0, 60, 0 }
        );
        /**/
    }
}
```

**Python : KinematicsIK**
```python
from underautomation.fanuc.kinematics.kinematics_utils import KinematicsUtils
from underautomation.fanuc.kinematics.dh_parameters import DhParameters
from underautomation.fanuc.common.cartesian_position import CartesianPosition

##
# OPW industrial robot
dh = DhParameters.from_arm_kinematic_model("ARCMate120iD")
target = CartesianPosition(x=800, y=0, z=450, w=180, p=0, r=90)
solutions = KinematicsUtils.inverse_kinematics(target, dh)

# CRX cobot with dual solutions
dh_crx = DhParameters.from_arm_kinematic_model("CRX10iAL")
target_crx = CartesianPosition(x=400, y=250, z=650, w=0, p=90, r=0)
crx_solutions = KinematicsUtils.inverse_kinematics(
    target_crx, dh_crx,
    include_duals=True,
    seed_joints=[0, -20, 40, 0, 60, 0]
)
##
```

### Solve inverse kinematics (IK) for a CRX cobot with dual solutions

### Build DH parameters from multiple sources

- **Built-in catalog**: `DhParameters.FromArmKinematicModel(ArmKinematicModels model)` gives you ready-to-use geometry for many
  Fanuc arms and cobots.
- **ROBOGUIDE library**: `DhParameters.FromDefFile(path)` parses robot definitions in `ProgramData/FANUC/ROBOGUIDE/Robot Library
`.
- **Controller variables**: `DhParameters.FromSymotnFile` and `DhParameters.FromMrrGrp` convert live `$MRR_GRP` or `symotn.va` d
  ata to reusable DH structures.
- **OPW data**: `DhParameters.FromOpwParameters` maps OPW parameters (meters) to Fanuc-style DH while keeping the kinematics cat
  egory consistent.

## Practical tips ✨

- **Offline safety**: Because all solvers are offline, you can iterate quickly without touching a robot controller.
- **Pose normalization**: `OpwKinematicsUtils.InverseKinematics` normalizes angles to `(-180, 180]` to match Fanuc expectations.
- **Seeding CRX searches**: Pass `seedJoints` to `CrxKinematicsUtils.InverseKinematics` to bias solutions near a known posture.
- **Matrix helpers**: `KinematicsUtils.Mul` multiplies 2D matrices if you need to compose transforms manually.

Happy path-planning! 🛠️

## Online Forward and Inverse Kinematics

> **Pro tip**: You don't need DH parameters or offline solvers to perform kinematics when you have a live connection! The robot controller automatically computes both joint and Cartesian representations for every position register. Simply write a position in one form (joints or Cartesian) via SNPX, then read the same register back to get the other form. This lets the controller handle FK and IK for you-no geometry data required. 🎯

### Forward kinematics using SNPX position registers

Leverage the controller's built-in FK solver by writing joint angles and reading back the Cartesian pose:

**C# : KinematicsOnline**
```csharp
using UnderAutomation.Fanuc;
using UnderAutomation.Fanuc.Common;

public class KinematicsOnline
{
    static void Main()
    {
        FanucRobot _robot = new FanucRobot();
        _robot.Connect("192.168.0.1");

        /**/
        // Forward kinematics via SNPX: joints → Cartesian
        JointsPosition jointsPosition = new JointsPosition(10, 12, 50, 20, 12, 16);
        _robot.Snpx.PositionRegisters.Write(1, jointsPosition);
        CartesianPosition cartesianPosition = _robot.Snpx.PositionRegisters.Read(1).CartesianPosition;

        // Inverse kinematics via SNPX: Cartesian → joints
        CartesianPosition targetPosition = new CartesianPosition() { X = 100, Y = 100, Z = 100 };
        targetPosition.Configuration.WristFlip = WristFlip.Flip;
        targetPosition.Configuration.ArmUpDown = ArmUpDown.Down;
        targetPosition.Configuration.ArmLeftRight = ArmLeftRight.Left;
        _robot.Snpx.PositionRegisters.Write(1, targetPosition);
        JointsPosition resultJoints = _robot.Snpx.PositionRegisters.Read(1).JointsPosition;
        /**/
    }
}
```

**Python : KinematicsOnline**
```python
from underautomation.fanuc.fanuc_robot import FanucRobot
from underautomation.fanuc.common.joints_position import JointsPosition
from underautomation.fanuc.common.cartesian_position import CartesianPosition

robot = FanucRobot()
robot.connect("192.168.0.1")

##
# Forward kinematics via SNPX: joints → Cartesian
joints_position = JointsPosition(10, 12, 50, 20, 12, 16)
robot.snpx.position_registers.write(1, joints_position)
cartesian_position = robot.snpx.position_registers.read(1).cartesian_position

# Inverse kinematics via SNPX: Cartesian → joints
target_position = CartesianPosition(x=100, y=100, z=100)
target_position.configuration.wrist_flip = "Flip"
target_position.configuration.arm_up_down = "Down"
target_position.configuration.arm_left_right = "Left"
robot.snpx.position_registers.write(1, target_position)
result_joints = robot.snpx.position_registers.read(1).joints_position
##
```

### Inverse kinematics using SNPX position registers

Let the controller solve IK by writing a Cartesian pose and reading back joint angles. You will only have 1 solution since the cartesian position contains the configuration.

## Demonstration

Have a look at : https://fanuc-kinematics.underautomation.com

> This is a 3D playground that lets you experiment with forward and inverse kinematics for Fanuc robots and cobots

![Fanuc Robot Simulator](https://raw.githubusercontent.com/underautomation/fanuc-kinematics.underautomation.com/refs/heads/main/.github/assets/screenshot.gif)

You can also take a look at the Winforms Desktop project source which implements all these features. I can be [downloaded here](/fanuc/download).

![IK FK](/fanuc/winforms/forward-inverse-kinematics.png)

### Core types at a glance

**Members of Kinematics.KinematicsUtils**
```csharp
public static class KinematicsUtils {
    // Compute FK for given joint angles (rad) and DH parameters
    public static CartesianPosition ForwardKinematics(double[] jointAnglesRad, DhParameters dhParameters)

    // Compute FK for given joint angles (deg) and DH parameters
    public static CartesianPosition ForwardKinematics(JointsPosition jointAnglesDeg, DhParameters parameters)

    // Compute all inverse kinematics solutions for a desired end effector pose.
    public static JointsPosition[] InverseKinematics(CartesianPosition position, DhParameters parameters)

    // Multiply two 4x4 homogeneous transformation matrices.
    public static double[,] Mul(double[,] A, double[,] B)
}
```

**Members of Kinematics.DhParameters**
```csharp
public class DhParameters : IDhParameters {
    // Initializes a new empty instance of <xref href="UnderAutomation.Fanuc.Kinematics.DhParameters" data-throw-if-not-resolved="false"></xref>.
    public DhParameters()

    // Initializes a new instance of <xref href="UnderAutomation.Fanuc.Kinematics.DhParameters" data-throw-if-not-resolved="false"></xref> with the specified values.
    public DhParameters(double d4, double d5, double d6, double a1, double a2, double a3)

    // Initializes a new instance of <xref href="UnderAutomation.Fanuc.Kinematics.DhParameters" data-throw-if-not-resolved="false"></xref> by copying from an existing <xref href="UnderAutomation.Fanuc.Kinematics.IDhParameters" data-throw-if-not-resolved="false"></xref>.
    public DhParameters(IDhParameters parameters)

    // DH parameter A1 (mm).
    public double A1 { get; set; }

    // DH parameter A2 (mm).
    public double A2 { get; set; }

    // DH parameter A3 (mm).
    public double A3 { get; set; }

    // DH parameter D4 (mm).
    public double D4 { get; set; }

    // DH parameter D5 (mm).
    public double D5 { get; set; }

    // DH parameter D6 (mm).
    public double D6 { get; set; }

    public override bool Equals(object obj)

    // Returns DH parameters from a known Arm Kinematic Model name. Returns null if not found in enum ArmKinematicModels.
    public static DhParameters FromArmKinematicModel(string modelName)

    // Returns DH parameters from a known Arm Kinematic Model.
    public static DhParameters FromArmKinematicModel(ArmKinematicModels model)

    // Loads DH parameters of each robots described in a ROBOGUIDE definition file (*.def). By default, this file is located in "C:\ProgramData\FANUC\ROBOGUIDE\Robot Library".
    public static DhParameters[] FromDefFile(string path)

    // Loads DH parameters of each robots described in a ROBOGUIDE definition file (*.def). By default, this file is located in "C:\ProgramData\FANUC\ROBOGUIDE\Robot Library".
    public static DhParameters[] FromDefFile(XDocument doc)

    // Loads DH parameters from parsed variable $MRR_GRP located in symotn.va.
    public static DhParameters FromMrrGrp(MrrGrpVariableType mrrGrp)

    // Creates DH parameters from OPW parameters (in meters)
    // C1 and B are ignored because B is always 0 and C1 is not used in the DH representation.
    public static DhParameters FromOpwParameters(double a1, double a2, double c2, double c3, double c4)

    // Loads DH parameters of each group from a parsed symotn.va file.
    public static DhParameters[] FromSymotnFile(SymotnFile file)

    public override int GetHashCode()

    // Gets the kinematics category determined from the DH parameter values.
    public KinematicsCategory KinematicsCategory { get; }

    // User-defined tag for associating additional data with this instance.
    public object Tag

    public override string ToString()
}
```

**Members of Kinematics.KinematicsCategory**
```csharp
public enum KinematicsCategory {
    // CRX collaborative robot kinematics.
    Crx = 1

    // Invalid or unsupported kinematics configuration.
    Invalid = 0

    // OPW (ortho-parallel wrist) kinematics for standard industrial robots.
    Opw = 2
}
```

**Members of Kinematics.IDhParameters**
```csharp
public interface IDhParameters {
    // DH parameter A1 (mm).
    double A1 { get; }

    // DH parameter A2 (mm).
    double A2 { get; }

    // DH parameter A3 (mm).
    double A3 { get; }

    // DH parameter D4 (mm).
    double D4 { get; }

    // DH parameter D5 (mm).
    double D5 { get; }

    // DH parameter D6 (mm).
    double D6 { get; }
}
```

**Members of Common.JointsPosition**
```csharp
public class JointsPosition {
    // Default constructor
    public JointsPosition()

    // Constructor with 6 joint values in degrees
    public JointsPosition(double j1Deg, double j2Deg, double j3Deg, double j4Deg, double j5Deg, double j6Deg)

    // Constructor with 9 joint values in degrees
    public JointsPosition(double j1Deg, double j2Deg, double j3Deg, double j4Deg, double j5Deg, double j6Deg, double j7Deg, double j8Deg, double j9Deg)

    // Constructor from an array of joint values in degrees
    public JointsPosition(double[] values)

    public override bool Equals(object obj)

    public override int GetHashCode()

    // Check if joints position is near to expected joints position with a tolerance value
    public static bool IsNear(JointsPosition j1, JointsPosition j2, double degreesTolerance)

    // Gets or sets the joint value at the specified index
    public double this[int i] { get; set; }

    // Joint 1 in degrees
    public double J1 { get; set; }

    // Joint 2 in degrees
    public double J2 { get; set; }

    // Joint 3 in degrees
    public double J3 { get; set; }

    // Joint 4 in degrees
    public double J4 { get; set; }

    // Joint 5 in degrees
    public double J5 { get; set; }

    // Joint 6 in degrees
    public double J6 { get; set; }

    // Joint 7 in degrees
    public double J7 { get; set; }

    // Joint 8 in degrees
    public double J8 { get; set; }

    // Joint 9 in degrees
    public double J9 { get; set; }

    public override string ToString()

    // Numeric values for each joints
    public double[] Values { get; }
}
```

**Members of Common.CartesianPosition**
```csharp
public class CartesianPosition : XYZPosition {
    // Default constructor
    public CartesianPosition()

    // Constructor with position and rotations
    public CartesianPosition(double x, double y, double z, double w, double p, double r)

    // Constructor with position, rotations and configuration
    public CartesianPosition(double x, double y, double z, double w, double p, double r, Configuration configuration)

    // Copy constructor
    public CartesianPosition(CartesianPosition position)

    // Constructor from an XYZ position with rotations
    public CartesianPosition(XYZPosition position, double w, double p, double r)

    // Position configuration
    public Configuration Configuration { get; }

    public override bool Equals(object obj)

    // Create a CartesianPosition with unknow configuration from a homogeneous rotation and translation 4x4 matrix
    public static CartesianPosition FromHomogeneousMatrix(double[,] R)

    public override int GetHashCode()

    // Check if two Cartesian positions are near each other within specified tolerances
    public static bool IsNear(CartesianPosition a, CartesianPosition b, double mmTolerance, double degreesTolerance)

    // Normalize an angle to the range ]-180, 180]
    public static double NormalizeAngle(double angle)

    // Normalize the W, P, R angles to the range ]-180, 180]
    public static void NormalizeAngles(CartesianPosition pose)

    // P rotation in degrees (Ry)
    public double P { get; set; }

    // R rotation in degrees (Rz)
    public double R { get; set; }

    // Convert position to a homogeneous rotation and translation 4x4 matrix
    public double[,] ToHomogeneousMatrix()

    public override string ToString()

    // W rotation in degrees (Rx)
    public double W { get; set; }
}
```