Motion system, position & kinematics

AbbController robot = new AbbController();
robot.Connect("192.168.0.1");
// Every mechanical unit of the system, with its activation state
foreach (MechanicalUnitItem unit in robot.Rws.MotionSystem.GetMechanicalUnits())
{
    Console.WriteLine($"{unit.Name} : {unit.Mode}, drive module {unit.DriveModule}");
}

// Everything the controller knows about one unit
MechanicalUnitInfo info = robot.Rws.MotionSystem.GetMechanicalUnit("ROB_1");
Console.WriteLine($"{info.Type}, task {info.TaskName}, status {info.Status}");
Console.WriteLine($"tool {info.ToolName}, work object {info.WorkObjectName}, payload {info.PayloadName}");
Console.WriteLine($"{info.Axes} axes, jog mode {info.JogMode}, frame {info.CoordinateSystem}");


// Axis by axis. Axes are numbered from 1.
int axisCount = robot.Rws.MotionSystem.GetAxisCount("ROB_1");

for (int axis = 1; axis <= axisCount; axis++)
{
    AxisInfo axisInfo = robot.Rws.MotionSystem.GetAxis("ROB_1", axis);
    Pose axisPose = robot.Rws.MotionSystem.GetAxisPose("ROB_1", axis);

    Console.WriteLine($"axis {axisInfo.Number} : {axisInfo.Status}, at {axisPose}");
}

// Where the base of the unit stands, in millimetres
BaseFrame baseFrame = robot.Rws.MotionSystem.GetBaseFrame("ROB_1");
Console.WriteLine($"base frame {baseFrame}, type {baseFrame.Type}");


// Changing a property of a unit needs the mastership of the motion domain.
// Give only the properties you want to change, leave the others null.
robot.Rws.Mastership.Request(MastershipDomain.Motion);

try
{
    robot.Rws.MotionSystem.SetMechanicalUnit("ROB_1",
                                             tool: "tGripper",
                                             jogMode: JogMode.Cartesian,
                                             coordinateSystem: CoordinateSystem.Base);
}
finally
{
    robot.Rws.Mastership.Release();
}

// The controller answers success even when it could not apply one of the properties.
// Read the unit back to see what it really did.
Console.WriteLine(robot.Rws.MotionSystem.GetMechanicalUnit("ROB_1").ToolName);
// Declaring where a base or an axis sits changes the calibration of the cell.
// The user account needs the matching UAS grant.

AbbController robot = new AbbController();
robot.Connect("192.168.0.1");
// 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}");
}
robot.Disconnect();
}

AbbController robot = new AbbController();
robot.Connect("192.168.0.1");
// SetPositionTarget really moves the robot to a cartesian target.
// Same preconditions as jogging: manual mode, motors on, local client, motion mastership.
RobTarget target = robot.Rws.MotionSystem.GetRobTarget("ROB_1");
target.Z += 10; // 10 mm up

robot.Rws.Mastership.Request(MastershipDomain.Motion);

try
{
    robot.Rws.MotionSystem.SetPositionTarget(target);
}
finally
{
    robot.Rws.Mastership.Release();
}


// SetMechanicalUnitPosition does not move anything: it places the unit at the given joint
// values. Only a virtual controller accepts it, a real one refuses the call.
JointTarget joints = new JointTarget(new RobotJoints(0, 0, 0, 0, 30, 0), new ExternalJoints());

robot.Rws.Mastership.Request(MastershipDomain.Motion);

try
{
    robot.Rws.MotionSystem.SetMechanicalUnitPosition("ROB_1", joints);
}
finally
{
    robot.Rws.Mastership.Release();
}
robot.Disconnect();
}

AbbController robot = new AbbController();
robot.Connect("192.168.0.1");
// Check the preconditions before asking the robot to move
OperationMode mode = robot.Rws.Panel.GetOperationMode();
ControllerState state = robot.Rws.Panel.GetControllerState();

if (mode == OperationMode.Automatic || state != ControllerState.MotorsOn)
{
    Console.WriteLine("Jogging is refused in this state");
    return;
}


robot.Rws.Mastership.Request(MastershipDomain.Motion);

try
{
    // Which unit the jogging commands apply to
    robot.Rws.MotionSystem.SetJoggingMechanicalUnit("ROB_1");

    // How the six values are read depends on the jog mode of that unit
    robot.Rws.MotionSystem.SetMechanicalUnit("ROB_1", jogMode: JogMode.AxisGroup1);

    // The change count of the last reading. The controller refuses a command
    // built on a state that has moved on since.
    MotionSystemInfo info = robot.Rws.MotionSystem.GetInfo();

    // One small step on axis 1, nothing on the other five
    RobotJoints step = new RobotJoints(100, 0, 0, 0, 0, 0);

    robot.Rws.MotionSystem.Jog(step, info.ChangeCount.Value, JogIncrementMode.Small);
}
finally
{
    robot.Rws.Mastership.Release();
}


// With JogIncrementMode.None the robot moves for as long as the command is repeated,
// so the loop itself is what stops the motion.
MotionSystemInfo current = robot.Rws.MotionSystem.GetInfo();
RobotJoints speed = new RobotJoints(50, 0, 0, 0, 0, 0);

for (int i = 0; i < 20; i++)
{
    robot.Rws.MotionSystem.Jog(speed, current.ChangeCount.Value, JogIncrementMode.None);
    System.Threading.Thread.Sleep(100);
}

// Some jogging requests are accepted by the controller and still not honoured.
// The error state says what went wrong.
MotionSystemErrorState error = robot.Rws.MotionSystem.GetErrorState();
Console.WriteLine($"{error.State}, {error.Count} error(s)");
robot.Disconnect();
}

AbbController robot = new AbbController();
robot.Connect("192.168.0.1");
// 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}");
}
// Set robotHoldsWorkObject to true when the tool is fixed in the cell and the robot
// carries the work object. Set logErrors to true to have the controller write an

AbbController robot = new AbbController();
robot.Connect("192.168.0.1");
// Collision detection while the unit is jogged
MotionSupervision jogging = robot.Rws.MotionSystem.GetMotionSupervision("ROB_1");
Console.WriteLine($"jogging supervision {jogging.Enabled}, sensitivity {jogging.Level} %");

// Collision detection while the unit follows a programmed path
PathSupervision path = robot.Rws.MotionSystem.GetPathSupervision("ROB_1");
Console.WriteLine($"path supervision {path.Enabled}, sensitivity {path.Level} %");


// Writing these four values needs the mastership of the motion domain.
// The lower the percentage, the sooner the controller reports a collision.
robot.Rws.Mastership.Request(MastershipDomain.Motion);

try
{
    robot.Rws.MotionSystem.SetMotionSupervisionMode("ROB_1", true);
    robot.Rws.MotionSystem.SetMotionSupervisionLevel("ROB_1", 80);

    robot.Rws.MotionSystem.SetPathSupervisionMode("ROB_1", true);
    robot.Rws.MotionSystem.SetPathSupervisionLevel("ROB_1", 80);
}
finally
{
    robot.Rws.Mastership.Release();
}


// Collision prediction stops the robot before it hits something the controller
// knows about. It is a separate setting, and it needs no mastership.
bool predicting = robot.Rws.MotionSystem.GetCollisionPredictionMode();

if (!predicting)
{
    // Refused when the collision detection option is not installed.
    // The error message then names the missing option.
    robot.Rws.MotionSystem.SetCollisionPredictionMode(true);
}
robot.Disconnect();
}

AbbController robot = new AbbController();
robot.Connect("192.168.0.1");
// Is the arm free to be pushed by hand right now
LeadThroughStatus status = robot.Rws.MotionSystem.GetLeadThrough("ROB_1");
Console.WriteLine(status);

// Switching it on releases the arm: the motors have to be on, and the robot
// has to support lead through. This call needs no mastership.
robot.Rws.MotionSystem.SetLeadThrough("ROB_1", true);

// Switching it off makes the arm hold its position again
robot.Rws.MotionSystem.SetLeadThrough("ROB_1", false);
robot.Disconnect();
}

AbbController robot = new AbbController();
robot.Connect("192.168.0.1");
// How each joint of the unit was calibrated
CalibrationInfo calibration = robot.Rws.MotionSystem.GetCalibrationInfo("ROB_1");
Console.WriteLine($"method {calibration.CalibrationMethodUsed}, {calibration.ExistingJointCount} joint(s)");

// The controller always answers with more joint slots than the unit has.
// The extra ones carry no name and are marked as not existing.
foreach (CalibrationJointInfo joint in calibration.Joints)
{
    if (!joint.Exists) continue;

    Console.WriteLine($"{joint.JointName} : factory {joint.FactoryCalibrationMethod}, now {joint.CurrentCalibrationMethod}");
}

// The name each joint carries, and the name of its calibration data
foreach (MotorCalibrationName name in robot.Rws.MotionSystem.GetMotorCalibrationNames("ROB_1"))
{
    Console.WriteLine($"joint {name.Number} : {name.JointName}, data {name.CalibrationName}");
}


// The calibration is stored twice: in the controller cabinet and in the robot itself.
// The two copies are meant to agree.
SmbData smb = robot.Rws.MotionSystem.GetSmbData("ROB_1");

Console.WriteLine($"cabinet calibration {smb.CabinetCalibrationStatus}");
Console.WriteLine($"robot calibration   {smb.RobotCalibrationStatus}");

if (smb.CabinetCalibrationStatus == SmbDataStatus.ValidNotEqual)
{
    Console.WriteLine("the two copies do not hold the same data");
}
robot.Disconnect();
}

AbbController robot = new AbbController();
robot.Connect("192.168.0.1");
// These four operations replace the calibration of an axis. RWS has no way to
// put the previous one back, so read the state first and be sure of the position
// the axis is standing in.
MechanicalUnitInfo unit = robot.Rws.MotionSystem.GetMechanicalUnit("ROB_1");

if (unit.Status != MechanicalUnitStatus.Synchronized)
{
    Console.WriteLine($"ROB_1 is {unit.Status}");
}

robot.Rws.Mastership.Request(MastershipDomain.Motion);

try
{
    // Teaches the controller how the rotor of the motor is oriented.
    // Needed once after a motor has been replaced.
    robot.Rws.MotionSystem.Commutate("ROB_1", 1);

    // Move the axis to its synchronization mark first: the controller stores
    // the position the axis is in right now.
    robot.Rws.MotionSystem.SynchronizeAxisRevolutionCounter("ROB_1", 1);
    robot.Rws.MotionSystem.UpdateRevolutionCounter("ROB_1", 1);

    // Takes the current position of the axis as its new calibration position
    robot.Rws.MotionSystem.FineCalibrate("ROB_1", 1);
}
finally
{
    robot.Rws.Mastership.Release();
}


// Copy one of the two calibration data stores over the other. The overwritten
// one is gone, so read both first and keep the good one.
robot.Rws.MotionSystem.SetSmbData("ROB_1", SmbDataTransfer.RobotToController);

// Erase one of them. The controller cannot recover it.
robot.Rws.MotionSystem.ClearSmbData("ROB_1", SmbDataMemory.Controller);
robot.Disconnect();
}