The RMP Motion Controller APIs
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
#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);
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);
robot.PathAccelerationSet(1000); // UserUnits per sec sqd
robot.PathVelocitySet(50); // UserUnits per sec
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);
//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;
}
}
}
static void CheckErrors(RapidCodeObject rsiObject)
Check if the RapidCodeObject has any errors.
Helper Functions for checking logged creation errors, starting the network, etc.
void UserLabelSet(const char *const userLabel)
Set the axis User defined Label.
Represents a single axis of motion control. This class provides an interface for commanding motion,...
Definition rsi.h:5664
void UnitsSet(LinearUnits units)
Set the units the built model will use.
Describes the mathematical kinematic model of a robot.
The Builder for a linear kinematic model. Constructs a single Linear Kinematic Model to use when crea...
void JointAdd(const LinearJointMapping &joint)
adds a joint to the model using the configuration specified within the LinearJoint structure.
Vector3d Position
The position component of the Pose.
Quaternion Orientation
The orientation component of the Pose.
Quaternion for representing rotations.
uint64_t MotionDoneWait()=0
Waits for a move to complete.
RapidVector< RobotPosition > PathPlannedPositionsGet(uint64_t startFrame, uint64_t frameCount)=0
Get Positions (see RobotPosition) representing the planned motion in cartesian space that will happen...
void PathVelocitySet(double unitsPerSecond)=0
Sets the target linear cartesian velocity for each move (UserUnits/second).
bool IsRunning()=0
Returns whether or not the robot is in motion (from executor or other source). Can be used to determi...
double PathLinearScalingGet() const =0
Gets scaling between input to path motion and output of path motion to the kinematic model.
static Robot * RobotCreate(MotionController *controller, MultiAxis *multiAxis, KinematicModelBuilder *builder, uint32_t motionFrameBufferSize)
Create a Robot object to use G-Code, path motion, etc.
void PathAccelerationSet(double unitsPerSecondSquared)=0
Sets the target acceleration for the machine (UserUnits/second^2). Should be set appropriately based ...
LinearUnits PathUnitsGet() const =0
Gets the units of path motion.
void PathArc(const Pose &target, double radius, RotationDirection direction)=0
Appends an arc on to the current on the PathPlaneGet() plane XY by default.
void PathUnitsSet(LinearUnits units)=0
Defines the units Cartesian Path Motion is in.
void PathLine(const Pose &target)=0
void EStop()=0
Commands a joint EStop and clears the loaded moves.
void PathProgrammingModeSet(PathMode mode)=0
Sets the programming mode (Relative/Absolute).
void Run()=0
Run the loaded path lines/arcs. The behavior is non-blocking. Use Robot.MotionDoneWait() to block.
Represents a collection of joints in Cartesian space with forward and inverse kinematics....
void ClearFaults()=0
Clears the MultiAxis fault then the Robot's error bit.
const KinematicModel & ModelGet()=0
Get the model this robot was created with Visit our Topic Page for more information.
static void RobotDelete(MotionController *controller, Robot *robot)
Delete a Robot.
A representation of the robot containing the Pose and the positions of the free axes.
Vector3d is used for three-dimensional points and vectors.
static constexpr int32_t AxisFrameBufferSizeDefault
The default value of the AxisFrameBufferSize, also the minimum allowable value.
Definition rsi.h:814
Represents the RMP soft motion controller. This class provides an interface to general controller con...
Definition rsi.h:762
void AxisRemoveAll()
Remove all axes from a MultiAxis group.s.
void AxesAdd(Axis **axes, int32_t axisCount)
void ClearFaults()
Clear all faults for an Axis or MultiAxis.
void AmpEnableSet(bool enable)
Enable all amplifiers.
void Abort()
Abort an axis.
PathMode
Motion types. For G-code use and Cartesian path motion.
CartesianAxis
This enum specifies which Cartesian axis a LinearJointMapping maps a robot joint to.
RotationDirection
Rotational directions about an axis.
LinearUnits
Unit types. For G-code use.
The Cartesian namespace.