Forward & Inverse Kinematics

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

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

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
}

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

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

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