using UnderAutomation.UniversalRobots.Kinematics;

// Get DH parameters
IUrDhParameters dhParameters = KinematicsUtils.GetDhParametersFromModel(RobotModelsExtended.Ur5e);

// Forward kinematics
double[] joints = new double[]{0.0, 0.1, 0.0, 0.2, -0.3, 0}; // 6 joints in radians
var fkResult = KinematicsUtils.ForwardKinematics(joints, dhParameters);
Pose cartesianPose = Pose.From4x4MatrixToRotationVector(fkResult.ToolTransform);

// Invert kinematics
Pose cartesianPose = Pose.From4x4MatrixToRotationVector(fkResult.ToolTransform);
var matrix = cartesianPose.FromRotationVectorTo4x4Matrix();
double[][] ikResults = KinematicsUtils.InverseKinematics(matrix, dhParameters); // list of joints angles