CGTP can read the current robot position in Cartesian or joint format, and perform forward and inverse kinematics directly on the controller.

## Read current position

Read the live position of the robot (requires firmware **V9.10+**):

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

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

        /**/
        // Read Cartesian position
        CartesianPosition cartesian = robot.Cgtp.ReadCartesianPosition();
        Console.WriteLine($"X={cartesian.X}, Y={cartesian.Y}, Z={cartesian.Z}");
        Console.WriteLine($"W={cartesian.W}, P={cartesian.P}, R={cartesian.R}");

        // Read joint position
        JointsPosition joints = robot.Cgtp.ReadJointPosition();
        Console.WriteLine($"J1={joints.J1}, J2={joints.J2}, J3={joints.J3}");

        // Multi-group
        CartesianPosition group2 = robot.Cgtp.ReadCartesianPosition(groupNum: 2);
        JointsPosition joints2 = robot.Cgtp.ReadJointPosition(groupNum: 2);
        /**/
    }
}
```

**Python : CgtpPositionKinematicsRead**
```python
from underautomation.fanuc.fanuc_robot import FanucRobot

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

##
# Read Cartesian position
cartesian = robot.cgtp.read_cartesian_position()
print(f"X={cartesian.x}, Y={cartesian.y}, Z={cartesian.z}")
print(f"W={cartesian.w}, P={cartesian.p}, R={cartesian.r}")

# Read joint position
joints = robot.cgtp.read_joint_position()
print(f"J1={joints.j1}, J2={joints.j2}, J3={joints.j3}")

# Multi-group
group2 = robot.cgtp.read_cartesian_position(group_num=2)
joints2 = robot.cgtp.read_joint_position(group_num=2)
##
```

### Multi-group

For controllers with multiple motion groups, specify the group number.

## Record current position into a program

`SetProgramPositionToCurrentCartesianPosition` writes the robot's current Cartesian position to a given position index inside a TP program. The updated position is returned.

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

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

        /**/
        // Set position index 1 of "MY_PROG" to the current Cartesian position
        CartesianPosition pos = robot.Cgtp.SetProgramPositionToCurrentCartesianPosition("MY_PROG", 1);
        Console.WriteLine($"Recorded: X={pos.X}, Y={pos.Y}, Z={pos.Z}");
        Console.WriteLine($"           W={pos.W}, P={pos.P}, R={pos.R}");

        // Specify a motion group (default is 1)
        CartesianPosition posGroup2 = robot.Cgtp.SetProgramPositionToCurrentCartesianPosition("MY_PROG", 2, groupNumber: 2);
        /**/
    }
}
```

**Python : CgtpPositionKinematicsSetToCurrent**
```python
from underautomation.fanuc.fanuc_robot import FanucRobot

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

##
# Set position index 1 of "MY_PROG" to the current Cartesian position
pos = robot.cgtp.set_program_position_to_current_cartesian_position("MY_PROG", 1)
print(f"Recorded: X={pos.x}, Y={pos.y}, Z={pos.z}")
print(f"           W={pos.w}, P={pos.p}, R={pos.r}")

# Specify a motion group (default is 1)
pos_group2 = robot.cgtp.set_program_position_to_current_cartesian_position("MY_PROG", 2, group_number=2)
##
```

## Write a specific position into a program

`SetProgramPosition` writes an arbitrary Cartesian or joint position to a position index (P[n]) inside a TP program. Only the first motion group is supported via CGTP. Requires firmware **V9.10+**.

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

public class CgtpPositionKinematicsSetPosition
{
    static void Main()
    {
        FanucRobot robot = new FanucRobot();

        ConnectionParameters parameters = new ConnectionParameters("192.168.0.1");
        parameters.Cgtp.Enable = true;

        robot.Connect(parameters);

        /**/
        // Write a Cartesian position to P[1] in "MY_PROG"
        // Only the first motion group is supported via CGTP
        var cartPosition = new Position(
            userFrame: 0,
            userTool: 1,
            jointsPosition: null,
            cartesianPosition: new ExtendedCartesianPosition(500, 200, 300, 0, 90, 0, 0, 0, 0)
        );
        robot.Cgtp.SetProgramPosition("MY_PROG", 1, cartPosition);

        // Write a joint position to P[2] in "MY_PROG"
        var jointPosition = new Position(
            userFrame: 0,
            userTool: 1,
            jointsPosition: new JointsPosition { J1 = 0, J2 = -30, J3 = 45, J4 = 0, J5 = -90, J6 = 0 },
            cartesianPosition: null
        );
        robot.Cgtp.SetProgramPosition("MY_PROG", 2, jointPosition);
        /**/
    }
}
```

**Python : CgtpPositionKinematicsSetPosition**
```python
from underautomation.fanuc.fanuc_robot import FanucRobot
from underautomation.fanuc.connection_parameters import ConnectionParameters
from underautomation.fanuc.common.position import Position
from underautomation.fanuc.common.extended_cartesian_position import ExtendedCartesianPosition
from underautomation.fanuc.common.joints_position import JointsPosition

robot = FanucRobot()
parameters = ConnectionParameters("192.168.0.1")
parameters.cgtp.enable = True
robot.connect(parameters)

##
# Write a Cartesian position to P[1] in "MY_PROG"
# Only the first motion group is supported via CGTP
cart_position = Position(0, 1, None, ExtendedCartesianPosition(500, 200, 300, 0, 90, 0, 0, 0, 0))
robot.cgtp.set_program_position("MY_PROG", 1, cart_position)

# Write a joint position to P[2] in "MY_PROG"
joint_position = Position(0, 1, JointsPosition(j1=0, j2=-30, j3=45, j4=0, j5=-90, j6=0), None)
robot.cgtp.set_program_position("MY_PROG", 2, joint_position)
##
```

## Online kinematics

Perform forward and inverse kinematics using the controller's own kinematic model. This guarantees the same results as the robot itself.


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

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

        /**/
        // Inverse kinematics: Cartesian → Joints
        CartesianPosition target = new CartesianPosition(500, 200, 300, 0, 90, 0);
        JointsPosition joints = robot.Cgtp.InvertKinematics(
            group: 1,
            cartesianPosition: target,
            userTool: 1,
            userFrame: 0
        );

        // Forward kinematics: Joints → Cartesian
        JointsPosition jointPos = new JointsPosition { J1 = 0, J2 = 0, J3 = 0, J4 = 0, J5 = -90, J6 = 0 };
        CartesianPosition cartPos = robot.Cgtp.ForwardKinematics(
            group: 1,
            jointPosition: jointPos,
            userTool: 1,
            userFrame: 0
        );
        /**/
    }
}
```

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

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

##
# Inverse kinematics: Cartesian → Joints
target = CartesianPosition(500, 200, 300, 0, 90, 0)
joints = robot.cgtp.invert_kinematics(
    group=1,
    cartesian_position=target,
    user_tool=1,
    user_frame=0
)

# Forward kinematics: Joints → Cartesian
joint_pos = JointsPosition(j1=0, j2=0, j3=0, j4=0, j5=-90, j6=0)
cart_pos = robot.cgtp.forward_kinematics(
    group=1,
    joint_position=joint_pos,
    user_tool=1,
    user_frame=0
)
##
```


## Complete example

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

public class CgtpPositionKinematics
{
  public static void Main()
  {
    FanucRobot robot = new FanucRobot();

    ConnectionParameters parameters = new ConnectionParameters("192.168.0.1");
    parameters.Cgtp.Enable = true;

    robot.Connect(parameters);

    /**/
    // Read current Cartesian position
    CartesianPosition cartesian = robot.Cgtp.ReadCartesianPosition();
    Console.WriteLine($"X={cartesian.X}, Y={cartesian.Y}, Z={cartesian.Z}");

    // Read current joint position
    JointsPosition joints = robot.Cgtp.ReadJointPosition();
    Console.WriteLine($"J1={joints.J1}, J2={joints.J2}, J3={joints.J3}");

    // Multi-group: read position of group 2
    CartesianPosition group2 = robot.Cgtp.ReadCartesianPosition(groupNum: 2);

    // Inverse kinematics: Cartesian -> Joints
    CartesianPosition target = new CartesianPosition(500, 200, 300, 0, 90, 0);
    JointsPosition ikResult = robot.Cgtp.InvertKinematics(
        group: 1,
        cartesianPosition: target,
        userTool: 1,
        userFrame: 0
    );

    // Forward kinematics: Joints -> Cartesian
    JointsPosition jointTarget = new JointsPosition { J1 = 0, J2 = 0, J3 = 0, J4 = 0, J5 = -90, J6 = 0 };
    CartesianPosition fkResult = robot.Cgtp.ForwardKinematics(
        group: 1,
        jointPosition: jointTarget,
        userTool: 1,
        userFrame: 0
    );

    // Write a position into a TP program (first motion group only)
    var progPosition = new Position(
        userFrame: 0,
        userTool: 1,
        jointsPosition: null,
        cartesianPosition: new ExtendedCartesianPosition(500, 200, 300, 0, 90, 0, 0, 0, 0)
    );
    robot.Cgtp.SetProgramPosition("MY_PROG", 1, progPosition);
    /**/
  }
}
```

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

# Create a robot instance
robot = FanucRobot()

# Configure connection parameters
parameters = ConnectionParameters("192.168.0.1")
parameters.cgtp.enable = True

# Connect to the robot
robot.connect(parameters)

##
# Read current Cartesian position
cartesian = robot.cgtp.read_cartesian_position()
print(f"X={cartesian.x}, Y={cartesian.y}, Z={cartesian.z}")

# Read current joint position
joints = robot.cgtp.read_joint_position()
print(f"J1={joints.j1}, J2={joints.j2}, J3={joints.j3}")

# Multi-group: read position of group 2
group2 = robot.cgtp.read_cartesian_position(group_num=2)

# Inverse kinematics: Cartesian -> Joints
target = CartesianPosition(500, 200, 300, 0, 90, 0)
ik_result = robot.cgtp.invert_kinematics(
    group=1,
    cartesian_position=target,
    user_tool=1,
    user_frame=0
)

# Forward kinematics: Joints -> Cartesian
joint_target = JointsPosition(j1=0, j2=0, j3=0, j4=0, j5=-90, j6=0)
fk_result = robot.cgtp.forward_kinematics(
    group=1,
    joint_position=joint_target,
    user_tool=1,
    user_frame=0
)

# Write a position into a TP program (first motion group only)
from underautomation.fanuc.common.position import Position
from underautomation.fanuc.common.extended_cartesian_position import ExtendedCartesianPosition
prog_position = Position(0, 1, None, ExtendedCartesianPosition(500, 200, 300, 0, 90, 0, 0, 0, 0))
robot.cgtp.set_program_position("MY_PROG", 1, prog_position)
##
```

## API reference

**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; }
}
```

**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; }
}
```