Inverse and Forward 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)
     public static JointsPosition[] InverseKinematics(CartesianPosition position, DhParameters parameters)
     public static double[,] Mul(double[,] A, double[,] B)
}

public class DhParameters : IDhParameters {
     public DhParameters()
     public DhParameters(double d4, double d5, double d6, double a1, double a2, double a3)
     public DhParameters(IDhParameters parameters)
     public double A1 { get; set; }
     public double A2 { get; set; }
     public double A3 { get; set; }
     public double D4 { get; set; }
     public double D5 { get; set; }
     public double D6 { get; set; }
 
    // 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 KinematicsCategory KinematicsCategory { get; }
     public object Tag
}

public enum KinematicsCategory {
     Crx = 1
     Invalid = 0
     Opw = 2
}

public interface IDhParameters {
     double A1 { get; }
     double A2 { get; }
     double A3 { get; }
     double D4 { get; }
     double D5 { get; }
     double D6 { get; }
}

public class JointsPosition {
     public JointsPosition()
     public JointsPosition(double j1Deg, double j2Deg, double j3Deg, double j4Deg, double j5Deg, double j6Deg)
     public JointsPosition(double j1Deg, double j2Deg, double j3Deg, double j4Deg, double j5Deg, double j6Deg, double j7Deg, double j8Deg, double j9Deg)
     public JointsPosition(double[] values)
 
    // Check if joints position is near to expected joints position with a tolerance value
    public static bool IsNear(JointsPosition j1, JointsPosition j2, double degreesTolerance)
     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)
     public CartesianPosition(double x, double y, double z, double w, double p, double r, Configuration configuration)
     public CartesianPosition(CartesianPosition position)
     public CartesianPosition(XYZPosition position, double w, double p, double r)
 
    // Position configuration
    public Configuration Configuration { get; }
 
    // Create a CartesianPosition with unknow configuration from a homogeneous rotation and translation 4x4 matrix
    public static CartesianPosition FromHomogeneousMatrix(double[,] R)
 
    // 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; }
}