Position & kinematics

Read current Cartesian and joint positions, and compute forward and inverse kinematics directly on the controller via CGTP.

Read current position
Multi-group
Record current position into a program
Write a specific position into a program
Online kinematics
Complete example
API reference

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+):

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

Click to see the full code

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.

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

Click to see the full code

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+.


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

Click to see the full code

Online kinematics

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

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

Click to see the full code

Complete example

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

API reference

Class

CartesianPositioninherits XYZWPRPosition

C#Python

Fanuc cartesian position and rotations

Member	Type	Description
CartesianPosition() Constructor		Default constructor
CartesianPosition(double, double, double, double, double, double) Constructor		Constructor with position and rotations
CartesianPosition(double, double, double, double, double, double, Configuration) Constructor		Constructor with position, rotations and configuration
CartesianPosition(CartesianPosition) Constructor		Copy constructor
CartesianPosition(XYZPosition, double, double, double) Constructor		Constructor from an XYZ position with rotations
Configuration Property	Configuration	Position configuration
Equals(object) Method	bool
FromHomogeneousMatrix(double[,]) Method static	CartesianPosition	Create a CartesianPosition with unknow configuration from a homogeneous rotation and translation 4x4 matrix R : Homogeneous 4x4 matrix
GetHashCode() Method	int
IsNear(CartesianPosition, CartesianPosition, double, double) Method static	bool	Check if two Cartesian positions are near each other within specified tolerances
NormalizeAngle(double) Method static	double	Normalize an angle to the range ]-180, 180]
NormalizeAngles(CartesianPosition) Method static	void	Normalize the W, P, R angles to the range ]-180, 180]
ToHomogeneousMatrix() Method	double[,]	Convert position to a homogeneous rotation and translation 4x4 matrix
ToString() Method	string

Class

JointsPosition

C#Python

Joints position in degrees

Member	Type	Description
JointsPosition() Constructor		Default constructor
JointsPosition(double, double, double, double, double, double) Constructor		Constructor with 6 joint values in degrees
JointsPosition(double, double, double, double, double, double, double, double, double) Constructor		Constructor with 9 joint values in degrees
JointsPosition(double[]) Constructor		Constructor from an array of joint values in degrees
this[int] Property	double	Gets or sets the joint value at the specified index
J1 Property	double	Joint 1 in degrees
J2 Property	double	Joint 2 in degrees
J3 Property	double	Joint 3 in degrees
J4 Property	double	Joint 4 in degrees
J5 Property	double	Joint 5 in degrees
J6 Property	double	Joint 6 in degrees
J7 Property	double	Joint 7 in degrees
J8 Property	double	Joint 8 in degrees
J9 Property	double	Joint 9 in degrees
Values Property read only	double[]	Numeric values for each joints
Equals(object) Method	bool
GetHashCode() Method	int
IsNear(JointsPosition, JointsPosition, double) Method static	bool	Check if joints position is near to expected joints position with a tolerance value
ToString() Method	string

Any question?

AbbVie

GE

Lockheed Martin

Microsoft

Nissan

Stellantis

Position & kinematics

Read current position

Multi-group

Record current position into a program

Write a specific position into a program

Online kinematics

Complete example

API reference