PathMotion.cs

Attention

See the following Concept pages for a detailed explanation of this sample: Path Motion.

Warning

This is a sample program to assist in the integration of the RMP motion controller with your application. It may not contain all of the logic and safety features that your application requires. We recommend that you wire an external hardware emergency stop (e-stop) button for safety when using our code sample apps. Doing so will help ensure the safety of you and those around you and will prevent potential injury or damage.

---

The sample apps assume that the system (network, axes, I/O) are configured prior to running the code featured in the sample app. See the Configuration page for more information.

 
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;
    }
  }
}