PathMotion.cs

 
using RSI.RapidCode.dotNET; // Import our RapidCode Library.
using NUnit.Framework;
using System;
using System.Linq;
using System.Threading;
 
#if DOXYGEN // RSI internal documentation use only
using RSI.RapidCode;
using RSI.RapidCode.Cartesian; 
#endif
 
 
[TestFixture]
[Category("Software")]
public class PathMotion : SampleAppTestBase
{
  [SetUp]
  public void Setup()
  {
    jointsMultiAxis?.AxisRemoveAll();
  }
 
  [TearDown]
  public void Teardown()
  {
    robot?.EStop();
    robot?.MotionDoneWait();
    robot?.ClearFaults();
    Robot.RobotDelete(controller, robot);
    robot = null;
 
    jointsMultiAxis?.Abort();
    Thread.Sleep(50);
    jointsMultiAxis?.ClearFaults();
  }
 
  private void VerifyModel(KinematicModel model, LinearUnits expectedUnits, string expectedName)
  {
    var actualUnits = model.UnitsGet();
    Assert.That(actualUnits, Is.EqualTo(expectedUnits), $"Expected model units to be '{expectedUnits}' but was '{actualUnits}' instead!");
    var actualName = model.NameGet();
    Assert.That(actualName, Is.EqualTo(expectedName), $"Expected model name to be '{expectedName}' but was '{actualName}' instead!");
  }
 
  [Test]
  public void GantryPrimeAxis()
  {
    prime_axis = z_axis;
    // We assume the axes have been confgured and homed properly prior this this program.
 
    const string xLabel = "X-Axis";
    const string yLabel = "Y-Axis";
    const string primeLabel = "Y-Prime";
 
    // Set the expected labels for each axis.
    x_axis.UserLabelSet(xLabel);
    y_axis.UserLabelSet(yLabel);
    prime_axis.UserLabelSet(primeLabel);
 
    // The joint index of each axis is the index within the MultiAxis object.
    // "X-Axis" has joint index 0
    // "Y-Axis" has joint index 1
    // "Y-Prime" has joint index 2
    Axis[] axes = new Axis[] { x_axis, y_axis, prime_axis };
    jointsMultiAxis.AxesAdd(axes, axes.Length);
 
    const LinearUnits units = LinearUnits.Millimeters;
    const string modelName = "RSI_XY_Yp";
    const double scaling = 1.0, offset = 0.0;
    LinearModelBuilder builder = new LinearModelBuilder(modelName);
    builder.UnitsSet(units);
    builder.JointAdd(new LinearJointMapping(0, CartesianAxis.X) { ExpectedLabel = xLabel,     Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(1, CartesianAxis.Y) { ExpectedLabel = yLabel,     Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(2, CartesianAxis.Y) { ExpectedLabel = primeLabel, Scaling = scaling, Offset = offset });
 
    // Create a Robot object where the y and y prime will be geared 1:1
    const int motionFrameBufferSize = 50; // This is the minimum valid motion frame buffer size.
    robot = Robot.RobotCreate(controller, jointsMultiAxis, builder, motionFrameBufferSize);
    HelperFunctions.CheckErrors(robot);
 
    VerifyModel(robot.ModelGet(), units, modelName);
  }
 
  [Test, Timeout(Constants.MAX_TEST_TIME)]
  public void SimplePathMotion()
  {
    // We assume the axes have been confgured and homed properly prior this this program.
    const string xLabel = "X-Axis";
    const string yLabel = "Y-Axis";
    const string zLabel = "Z-Axis";
    const string aLabel = "A-Axis";
    const string bLabel = "B-Axis";
    const string cLabel = "C-Axis";
 
    // Set the expected labels for each axis.
    x_axis.UserLabelSet(xLabel);
    y_axis.UserLabelSet(yLabel);
    z_axis.UserLabelSet(zLabel);
    a_axis.UserLabelSet(aLabel);
    b_axis.UserLabelSet(bLabel);
    c_axis.UserLabelSet(cLabel);
 
    // The joint index of each axis is the index within the MultiAxis object.
    // "X-Axis" has joint index 0
    // "Y-Axis" has joint index 1
    // "Z-Axis" has joint index 2
    // "A-Axis" has joint index 3
    // "B-Axis" has joint index 4
    // "C-Axis" has joint index 5
    Axis[] axes = new Axis[] { x_axis, y_axis, z_axis, a_axis, b_axis, c_axis };
    jointsMultiAxis.AxesAdd(axes, axes.Length);
    jointsMultiAxis.ClearFaults();
 
    const LinearUnits units = LinearUnits.Meters;
    const string modelName = "RSI_XYZABC_Meters";
    const double scaling = 1.0, offset = 0.0;
    LinearModelBuilder builder = new LinearModelBuilder(modelName);
    builder.UnitsSet(units);
    builder.JointAdd(new LinearJointMapping(0, CartesianAxis.X)     { ExpectedLabel = xLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(1, CartesianAxis.Y)     { ExpectedLabel = yLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(2, CartesianAxis.Z)     { ExpectedLabel = zLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(3, CartesianAxis.Roll)  { ExpectedLabel = aLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(4, CartesianAxis.Pitch) { ExpectedLabel = bLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(5, CartesianAxis.Yaw)   { ExpectedLabel = cLabel, Scaling = scaling, Offset = offset });
 
    // Create a Robot object with a multi axis containing all joints and the kinematic type
    robot = Robot.RobotCreate(controller, jointsMultiAxis, builder, MotionController.AxisFrameBufferSizeDefault);
    HelperFunctions.CheckErrors(robot);
 
    robot.PathAccelerationSet(1000);  // UserUnits per sec sqd
    robot.PathVelocitySet(50);        // UserUnits per sec 
 
    robot.PathProgrammingModeSet(PathMode.Absolute);
    robot.PathLine(new Pose(1, 1, 0));
    robot.PathArc(new Pose(0, 2, 0), new Vector3d(0, 1, 0), RotationDirection.Clockwise);
 
    jointsMultiAxis.ClearFaults();
    jointsMultiAxis.AmpEnableSet(true);
    robot.Run(); // Starts the motion. Calling with the false arguement for non blocking behavior
    bool isRunning = robot.IsRunning();
 
    VerifyModel(robot.ModelGet(), units, modelName);
  }
 
  [Test]
  public void ChangingUnits()
  {
    // We assume the axes have been confgured and homed properly prior this this program.
    const string xLabel = "X-Axis";
    const string yLabel = "Y-Axis";
    const string zLabel = "Z-Axis";
    const string aLabel = "A-Axis";
    const string bLabel = "B-Axis";
    const string cLabel = "C-Axis";
 
    // Set the expected labels for each axis.
    x_axis.UserLabelSet(xLabel);
    y_axis.UserLabelSet(yLabel);
    z_axis.UserLabelSet(zLabel);
    a_axis.UserLabelSet(aLabel);
    b_axis.UserLabelSet(bLabel);
    c_axis.UserLabelSet(cLabel);
 
    // The joint index of each axis is the index within the MultiAxis object.
    // "X-Axis" has joint index 0
    // "Y-Axis" has joint index 1
    // "Z-Axis" has joint index 2
    // "A-Axis" has joint index 3
    // "B-Axis" has joint index 4
    // "C-Axis" has joint index 5
    Axis[] axes = new Axis[] { x_axis, y_axis, z_axis, a_axis, b_axis, c_axis };
    jointsMultiAxis.AxesAdd(axes, axes.Length);
 
    const LinearUnits units = LinearUnits.Millimeters;
    const string modelName = "RSI_XYZABC_Millimeters";
    const double scaling = 1.0, offset = 0.0;
    LinearModelBuilder builder = new LinearModelBuilder(modelName);
    builder.UnitsSet(units);
    builder.JointAdd(new LinearJointMapping(0, CartesianAxis.X)     { ExpectedLabel = xLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(1, CartesianAxis.Y)     { ExpectedLabel = yLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(2, CartesianAxis.Z)     { ExpectedLabel = zLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(3, CartesianAxis.Roll)  { ExpectedLabel = aLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(4, CartesianAxis.Pitch) { ExpectedLabel = bLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(5, CartesianAxis.Yaw)   { ExpectedLabel = cLabel, Scaling = scaling, Offset = offset });
 
    // Create a Robot object with a multi axis containing all joints and the kinematic type
    robot = Robot.RobotCreate(controller, jointsMultiAxis, builder, MotionController.AxisFrameBufferSizeDefault);
    HelperFunctions.CheckErrors(robot);
 
    //NOTE: to use the above kinematic model you must have a gantry (linear 1:1 kinematics) and each linear axis must have its user units scaled to millimeters
 
    robot.PathAccelerationSet(10); //Sets the PATH acceleration to 10 Millimeters per second sqd
    robot.PathVelocitySet(10);     //Sets the PATH velocity to 10 Millimeters per second
 
    Console.WriteLine(robot.PathUnitsGet());//This will print the int value for the enum for Millimeters 
 
    robot.PathUnitsSet(LinearUnits.Centimeters);
 
    robot.PathAccelerationSet(10); //Sets the PATH acceleration to 10 Centimeters per second sqd
    robot.PathVelocitySet(10);     //Sets the PATH velocity to 10 Centimeters per second
 
    Console.WriteLine(robot.PathLinearScalingGet());//Will print the scale factor to convert CM to MM
 
    //NOTE: you can also set this directly. Doing so will result in PathUnitsGet() returning NONE
 
    VerifyModel(robot.ModelGet(), units, modelName);
  }
 
  public void PathPoint()
  {
    const ulong frameCount = 500;
    ulong totalFrames = frameCount;
    ulong startFrame = 0;
 
    while (totalFrames == frameCount)
    {
      RapidVectorRobotPosition positions = robot.PathPlannedPositionsGet(startFrame, frameCount);
      totalFrames = positions.Size();
 
      foreach (RobotPosition position in positions.ToArray())
      {
        Pose pose = position.Pose;
        Vector3d spatial = pose.Position;
        double xCoord = spatial.X, yCoord = spatial.Y, zCoord = spatial.Z;
        Quaternion orientation = pose.Orientation;
      }
 
      //Add code here to create the rendering out of the positions you have collected.
      //Use your prefered 3d library. 
      //Example:
      //vector3 = new Vector3D(xPosition, yPosition, zPosition);
      //linePositions.Add(Vector3DToVector3(vector3));
 
      startFrame += totalFrames;
    }
  }
}