Get the robot position

// Where the tool is, in millimetres, with the axis configuration and the external axes
RobTarget target = robot.Rws.MotionSystem.GetRobTarget("ROB_1");
Console.WriteLine($"X={target.X} Y={target.Y} Z={target.Z}");
Console.WriteLine($"orientation {target.Orientation}");
Console.WriteLine($"configuration {target.Configuration}");
Console.WriteLine($"external axes {target.ExternalAxes}");

// The same reading in another frame, with a given tool and work object
RobTarget inWorld = robot.Rws.MotionSystem.GetRobTarget("ROB_1", CoordinateSystem.World,
                                                        tool: "tGripper", workObject: "wobj0");
Console.WriteLine(inWorld);


// The joint values, robot axes in degrees
JointTarget joints = robot.Rws.MotionSystem.GetJointTarget("ROB_1");
Console.WriteLine($"axis 1 = {joints.RobotAxes.Axis1} deg");
Console.WriteLine($"axis 2 = {joints.RobotAxes.Axis2} deg");

// An external axis the system does not define comes back as ExternalJoints.NotInUse
if (joints.ExternalAxes.AxisA != ExternalJoints.NotInUse)
{
    Console.WriteLine($"external axis A = {joints.ExternalAxes.AxisA}");
}

// alwaysRead asks the controller to measure again instead of answering with the value it holds
JointTarget measured = robot.Rws.MotionSystem.GetJointTarget("ROB_1", alwaysRead: true);
Console.WriteLine(measured);


// Cartesian position without the external axes. ExternalAxes is null here.
RobTarget cartesian = robot.Rws.MotionSystem.GetCartesianPosition("ROB_1");
Console.WriteLine(cartesian);

// The raw values of the measurement system of the unit
RobotJoints physical = robot.Rws.MotionSystem.GetPhysicalJoints("ROB_1");
Console.WriteLine(physical);


// The same two positions seen from a RAPID task instead of a mechanical unit
RobTarget taskTarget = robot.Rws.Rapid.GetRobTarget("T_ROB1");
JointTarget taskJoints = robot.Rws.Rapid.GetJointTarget("T_ROB1");

// Which external joints of the task carry a real value
RapidExternalJointStates states = robot.Rws.Rapid.GetExternalJointStates("T_ROB1");
Console.WriteLine($"external joint 1 : {states.Joint1}");

// The units the task can move
foreach (RapidMechanicalUnitItem unit in robot.Rws.Rapid.GetMechanicalUnits("T_ROB1"))
{
    Console.WriteLine($"{unit.Name} : {unit.Type}, {unit.Mode}");
}

// These four calculations work in metres and radians, not in millimetres and degrees.

// Tool relative to the mounting flange. Here the flange itself, with no rotation.
Pose toolFrame = new Pose(0, 0, 0, new Quaternion(1, 0, 0, 0));

// Forward kinematics: where the tool would be for these joint values
JointTarget joints = new JointTarget(new RobotJoints(0, 0, 0, 0, 0.5, 0), new ExternalJoints());

RobTarget pose = robot.Rws.MotionSystem.GetPoseFromJoints("ROB_1", toolFrame, joints);
Console.WriteLine($"tool at {pose.X} {pose.Y} {pose.Z} metres, configuration {pose.Configuration}");


// Inverse kinematics: which joint values put the tool at that pose.
// previousJoints decides between the solutions the pose admits.
JointTarget solution = robot.Rws.MotionSystem.GetJointsFromPose("ROB_1", pose, new ExternalJoints(),
                                                               toolFrame, joints, pose.Configuration);
Console.WriteLine($"axis 5 = {solution.RobotAxes.Axis5} rad");

// The controller has a second calculation for the same question. It does not always
// pick the same solution.
JointTarget other = robot.Rws.MotionSystem.GetJointsFromCartesian("ROB_1", pose, new ExternalJoints(),
                                                                 toolFrame, joints, pose.Configuration);
Console.WriteLine(other);


// Every way of reaching the pose. A six axis robot usually has eight.
JointSolution[] solutions = robot.Rws.MotionSystem.GetAllJointSolutions("ROB_1", pose, new ExternalJoints(),
                                                                       toolFrame, pose.Configuration);

foreach (JointSolution s in solutions)
{
    Console.WriteLine($"{s.Configuration} : {s.RobotAxes}");
}

// The readings answer in millimetres and degrees
    RobTarget reading = robot.Rws.MotionSystem.GetRobTarget("ROB_1");
    JointTarget joints = robot.Rws.MotionSystem.GetJointTarget("ROB_1");

    Console.WriteLine($"{reading.X} mm, axis 5 = {joints.RobotAxes.Axis5} deg");

    // The four kinematics calculations work in metres and radians, in both directions.
    // Convert the joints before sending them.
    JointTarget inRadians = new JointTarget(ToRadians(joints.RobotAxes), joints.ExternalAxes);
    Pose toolFrame = new Pose(0, 0, 0, new Quaternion(1, 0, 0, 0));

    RobTarget computed = robot.Rws.MotionSystem.GetPoseFromJoints("ROB_1", toolFrame, inRadians);

    // And convert the answer back to millimetres to compare it with the reading
    Console.WriteLine($"{computed.X * 1000} mm computed, {reading.X} mm read");
    

static RobotJoints ToRadians(RobotJoints degrees)
{
    double f = Math.PI / 180.0;

    return new RobotJoints(degrees.Axis1 * f, degrees.Axis2 * f, degrees.Axis3 * f,
                           degrees.Axis4 * f, degrees.Axis5 * f, degrees.Axis6 * f);
}

static RobotJoints ToDegrees(RobotJoints radians)
{
    double f = 180.0 / Math.PI;

    return new RobotJoints(radians.Axis1 * f, radians.Axis2 * f, radians.Axis3 * f,
                           radians.Axis4 * f, radians.Axis5 * f, radians.Axis6 * f);
}