Fanuc SDK documentation
Position & kinematics
Read current Cartesian and joint positions, and compute forward and inverse kinematics directly on the controller via CGTP.
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+):
// Read Cartesian positionCartesianPosition 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 positionJointsPosition joints = robot.Cgtp.ReadJointPosition();Console.WriteLine($"J1={joints.J1}, J2={joints.J2}, J3={joints.J3}");// Multi-groupCartesianPosition group2 = robot.Cgtp.ReadCartesianPosition(groupNum: 2);JointsPosition joints2 = robot.Cgtp.ReadJointPosition(groupNum: 2);
Multi-group
For controllers with multiple motion groups, specify the group number.
Online kinematics
Perform forward and inverse kinematics using the controller's own kinematic model. This guarantees the same results as the robot itself.
// Inverse kinematics: Cartesian → JointsCartesianPosition 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 → CartesianJointsPosition 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);
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 positionCartesianPosition cartesian = robot.Cgtp.ReadCartesianPosition();Console.WriteLine($"X={cartesian.X}, Y={cartesian.Y}, Z={cartesian.Z}");// Read current joint positionJointsPosition joints = robot.Cgtp.ReadJointPosition();Console.WriteLine($"J1={joints.J1}, J2={joints.J2}, J3={joints.J3}");// Multi-group: read position of group 2CartesianPosition group2 = robot.Cgtp.ReadCartesianPosition(groupNum: 2);// Inverse kinematics: Cartesian -> JointsCartesianPosition 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 -> CartesianJointsPosition 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);/**/}}
API reference
Members of Common.CartesianPosition :public class CartesianPosition : XYZPosition {// Default constructorpublic CartesianPosition()// Constructor with position and rotationspublic CartesianPosition(double x, double y, double z, double w, double p, double r)// Constructor with position, rotations and configurationpublic CartesianPosition(double x, double y, double z, double w, double p, double r, Configuration configuration)// Copy constructorpublic CartesianPosition(CartesianPosition position)// Constructor from an XYZ position with rotationspublic CartesianPosition(XYZPosition position, double w, double p, double r)// Position configurationpublic Configuration Configuration { get; }public override bool Equals(object obj)// Create a CartesianPosition with unknow configuration from a homogeneous rotation and translation 4x4 matrixpublic static CartesianPosition FromHomogeneousMatrix(double[,] R)public override int GetHashCode()// Check if two Cartesian positions are near each other within specified tolerancespublic 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 matrixpublic double[,] ToHomogeneousMatrix()public override string ToString()// W rotation in degrees (Rx)public double W { get; set; }}
public class JointsPosition {// Default constructorpublic JointsPosition()// Constructor with 6 joint values in degreespublic JointsPosition(double j1Deg, double j2Deg, double j3Deg, double j4Deg, double j5Deg, double j6Deg)// Constructor with 9 joint values in degreespublic 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 degreespublic 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 valuepublic static bool IsNear(JointsPosition j1, JointsPosition j2, double degreesTolerance)// Gets or sets the joint value at the specified indexpublic double this[int i] { get; set; }// Joint 1 in degreespublic double J1 { get; set; }// Joint 2 in degreespublic double J2 { get; set; }// Joint 3 in degreespublic double J3 { get; set; }// Joint 4 in degreespublic double J4 { get; set; }// Joint 5 in degreespublic double J5 { get; set; }// Joint 6 in degreespublic double J6 { get; set; }// Joint 7 in degreespublic double J7 { get; set; }// Joint 8 in degreespublic double J8 { get; set; }// Joint 9 in degreespublic double J9 { get; set; }public override string ToString()// Numeric values for each jointspublic double[] Values { get; }}