This page shows how to compute the forward and inverse kinematics of a Staubli robot with the SDK. The controller computes them with the geometry of the real arm: the result is the one that the CS8 or CS9 controller uses for its own moves.

## Forward kinematics

`ForwardKinematics(robot, joints)` returns the flange frame for joint values in radians, and the configuration of the arm for these joints.

**C# : KinematicsForward**
```csharp
using UnderAutomation.Staubli;
using UnderAutomation.Staubli.Soap.Data;

public class KinematicsForward
{
    static void Main()
    {
        var controller = new StaubliController();
        controller.Connect("192.168.0.254");

        /**/
        // Joint values in radians, here the current ones
        double[] joints = controller.Soap.GetCurrentJointPosition(robot: 0);

        // The controller computes the flange frame for these joints
        IForwardKinematics fk = controller.Soap.ForwardKinematics(robot: 0, joints);

        // Position of the frame origin, and its orientation as a rotation matrix
        Frame frame = fk.Position;
        Console.WriteLine($"P = [{frame.Px}, {frame.Py}, {frame.Pz}]");

        // Configuration of the arm for this position (shoulder, elbow, wrist)
        Config config = fk.Config;
        Console.WriteLine(config);
        /**/

        controller.Disconnect();
    }
}
```

**Python : KinematicsForward**
```python
from underautomation.staubli.staubli_controller import StaubliController

controller = StaubliController()
controller.connect("192.168.0.254")

##
# Joint values in radians, here the current ones
joints = controller.soap.get_current_joint_position(0)

# The controller computes the flange frame for these joints
fk = controller.soap.forward_kinematics(0, joints)

# Position of the frame origin, and its orientation as a rotation matrix
frame = fk.position
print(f"P = [{frame.px}, {frame.py}, {frame.pz}]")

# Configuration of the arm for this position (shoulder, elbow, wrist)
config = fk.config
print(config)
##

controller.disconnect()
```

**Matlab : KinematicsForward**
```matlab
NET.addAssembly(fullfile(pwd, 'net48', 'UnderAutomation.Staubli.dll'));
import UnderAutomation.Staubli.*

controller = StaubliController();
controller.Connect('192.168.0.254');

%%
% Joint values in radians. A Matlab vector is passed as a .NET array.
joints = [0, 0, pi/2, 0, pi/2, 0];

% The controller computes the flange frame for these joints
fk = controller.Soap.ForwardKinematics(0, joints);

% Position of the frame origin
frame = fk.Position;
fprintf('P = [%.4f, %.4f, %.4f]\n', frame.Px, frame.Py, frame.Pz);

% Rotation matrix of the frame, columns N, O, A
R = [frame.Nx frame.Ox frame.Ax; frame.Ny frame.Oy frame.Ay; frame.Nz frame.Oz frame.Az];
%%

controller.Disconnect();
```

The result is a `Frame`: the origin `Px`, `Py`, `Pz` in meters, and a rotation matrix given by its three columns:

| Column | Properties       | Axis of the frame     |
| ------ | ---------------- | --------------------- |
| N      | `Nx`, `Ny`, `Nz` | X axis                |
| O      | `Ox`, `Oy`, `Oz` | Y axis                |
| A      | `Ax`, `Ay`, `Az` | Z axis, approach axis |

## Configuration

A Cartesian position can be reached with several joint positions: shoulder on the left or on the right, elbow up or down, wrist flipped or not. The `Config` object selects one of them. It has one part per type of arm:

- `AnthroConfig` for 6 axis arms: `Shoulder` (`Lefty`, `Righty`), `Elbow` and `Wrist` (`Positive`, `Negative`);
- `ScaraConfig` for SCARA arms: `Shoulder`;
- `VrbxConfig` for the other kinematics.

Each value can also be `Same` (keep the current configuration) or `Free` (any configuration).

## Inverse kinematics

`ReverseKinematics(robot, joints, target, config, jointRange)` returns the joints for a target frame. The controller starts from `joints`, keeps the configuration `config`, and rejects a solution out of `jointRange`.

**C# : KinematicsInverse**
```csharp
using UnderAutomation.Staubli;
using UnderAutomation.Staubli.Soap.Data;

public class KinematicsInverse
{
    static void Main()
    {
        var controller = new StaubliController();
        controller.Connect("192.168.0.254");

        /**/
        double[] current = controller.Soap.GetCurrentJointPosition(robot: 0);
        JointRange range = controller.Soap.GetJointRange(robot: 0);

        // Target: the current flange frame, 50 mm higher
        IForwardKinematics fk = controller.Soap.ForwardKinematics(robot: 0, current);
        Frame target = fk.Position;
        target.Pz += 0.050;

        // Keep the current configuration of the arm
        IReverseKinematics ik = controller.Soap.ReverseKinematics(robot: 0, current, target, fk.Config, range);

        if (ik.Result == ReversingResult.Success)
            Console.WriteLine(string.Join(", ", ik.Joint));
        else
            Console.WriteLine($"No solution: {ik.Result}"); // OutOfWorkspace, JointOutOfRange...
        /**/

        controller.Disconnect();
    }
}
```

**Python : KinematicsInverse**
```python
from underautomation.staubli.staubli_controller import StaubliController
from underautomation.staubli.soap.data.reversing_result import ReversingResult

controller = StaubliController()
controller.connect("192.168.0.254")

##
current = controller.soap.get_current_joint_position(0)
joint_range = controller.soap.get_joint_range(0)

# Target: the current flange frame, 50 mm higher
fk = controller.soap.forward_kinematics(0, current)
target = fk.position
target.pz += 0.050

# Keep the current configuration of the arm
ik = controller.soap.reverse_kinematics(0, current, target, fk.config, joint_range)

if ik.result == ReversingResult.Success:
    print(list(ik.joint))
else:
    print(f"No solution: {ik.result.name}")  # OutOfWorkspace, JointOutOfRange...
##

controller.disconnect()
```

Always test `Result` before you use the joints:

| `ReversingResult`      | Meaning                                 |
| ---------------------- | --------------------------------------- |
| `Success`              | `Joint` holds the solution              |
| `OutOfWorkspace`       | The target is out of reach              |
| `JointOutOfRange`      | The solution is out of the joint ranges |
| `InvalidConfiguration` | No solution with this configuration     |
| `NoConvergence`        | The computation did not find a solution |

## Try it in the demo application



## Reference

**Methods of SoapClientBase**
```csharp
// Calculate the forward kinematics of a robot based on its joint positions
IForwardKinematics ForwardKinematics(int robot, double[] joints);

// Calculate the reverse kinematics of a robot to reach a target position and orientation
IReverseKinematics ReverseKinematics(int robot, double[] joint, Frame target, Config config, JointRange jointRange);
```

Every method also exists in an asynchronous version, with the same name followed by `Async` and an optional `CancellationToken`.

**Members of Soap.Data.IForwardKinematics**
```csharp
public interface IForwardKinematics {
    // Robot configuration associated with the computed position.
    Config Config { get; }

    // Cartesian position resulting from the forward kinematics computation.
    Frame Position { get; }
}
```

**Members of Soap.Data.IReverseKinematics**
```csharp
public interface IReverseKinematics {
    // Joint angles resulting from the reverse kinematics computation.
    double[] Joint { get; }

    // Result code indicating the outcome of the reverse kinematics computation.
    ReversingResult Result { get; }
}
```

**Members of Soap.Data.ReversingResult**
```csharp
public enum ReversingResult {
    // The specified configuration is invalid.
    InvalidConfiguration = 4

    // Invalid error code returned.
    InvalidErrorCode = 8

    // The specified orientation is invalid.
    InvalidOrientation = 5

    // The computed joint position is out of range.
    JointOutOfRange = 2

    // The algorithm did not converge to a solution.
    NoConvergence = 1

    // The target is outside the robot workspace.
    OutOfWorkspace = 3

    // Reverse kinematics succeeded.
    Success = 0

    // The frame is unconstrained.
    UnconstrainedFrame = 7

    // The robot kinematics type is not supported.
    UnsupportedKinematics = 6
}
```

**Members of Soap.Data.Frame**
```csharp
public class Frame {
    // Default constructor.
    public Frame()

    // X component of the local Z-axis vector (Approach X).
    public double Ax { get; set; }

    // Y component of the local Z-axis vector (Approach Y).
    public double Ay { get; set; }

    // Z component of the local Z-axis vector (Approach Z).
    public double Az { get; set; }

    public override bool Equals(object obj)

    public override int GetHashCode()

    // X component of the local X-axis vector (Normal X).
    public double Nx { get; set; }

    // Y component of the local X-axis vector (Normal Y).
    public double Ny { get; set; }

    // Z component of the local X-axis vector (Normal Z).
    public double Nz { get; set; }

    // X component of the local Y-axis vector (Orientation X).
    public double Ox { get; set; }

    // Y component of the local Y-axis vector (Orientation Y).
    public double Oy { get; set; }

    // Z component of the local Y-axis vector (Orientation Z).
    public double Oz { get; set; }

    // X coordinate of the frame's origin in global space (Pose X).
    public double Px { get; set; }

    // Y coordinate of the frame's origin in global space (Pose Y).
    public double Py { get; set; }

    // Z coordinate of the frame's origin in global space (Pose Z).
    public double Pz { get; set; }

    public override string ToString()
}
```

**Members of Soap.Data.Config**
```csharp
public class Config {
    // Initializes a new instance of the <xref href="UnderAutomation.Staubli.Soap.Data.Config" data-throw-if-not-resolved="false"></xref> class.
    public Config()

    // Anthropomorphic robot configuration.
    public AnthroConfig AnthroConfig { get; set; }

    public override bool Equals(object obj)

    public override int GetHashCode()

    // SCARA robot configuration.
    public ScaraConfig ScaraConfig { get; set; }

    public override string ToString()

    // VRBX robot configuration.
    public VrbxConfig VrbxConfig { get; set; }
}
```

**Members of Soap.Data.AnthroConfig**
```csharp
public class AnthroConfig {
    // Initializes a new instance of the <xref href="UnderAutomation.Staubli.Soap.Data.AnthroConfig" data-throw-if-not-resolved="false"></xref> class.
    public AnthroConfig()

    // Elbow configuration.
    public PositiveNegativeConfig Elbow { get; set; }

    public override bool Equals(object obj)

    public override int GetHashCode()

    // Shoulder configuration.
    public ShoulderConfig Shoulder { get; set; }

    public override string ToString()

    // Wrist configuration.
    public PositiveNegativeConfig Wrist { get; set; }
}
```

**Members of Soap.Data.ScaraConfig**
```csharp
public class ScaraConfig {
    // Initializes a new instance of the <xref href="UnderAutomation.Staubli.Soap.Data.ScaraConfig" data-throw-if-not-resolved="false"></xref> class.
    public ScaraConfig()

    public override bool Equals(object obj)

    public override int GetHashCode()

    // Shoulder configuration.
    public ShoulderConfig Shoulder { get; set; }

    public override string ToString()
}
```