path-3d-points.cs

Note

See Motion: Path 📜 for a detailed explanation of this sample code.

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.

/*  This sample demonstrates how to get 3D path points for rendering visualization.
    Shows how to retrieve planned positions from a Robot path for use with 3D graphics libraries
    or visualization tools.
*/
 
using RSI.RapidCode; // RSI.RapidCode.dotNET;
 
Console.WriteLine("📜 Motion: Path 3D Rendering");
 
// get rmp objects
MotionController controller = MotionController.Get();
 
try
{
    Helpers.CheckErrors(controller);

    // set robot axis labels
    const string xLabel = "X-Axis";
    const string yLabel = "Y-Axis";
    const string zLabel = "Z-Axis";
 
    // get the 3 axis needed for XYZ robot
    Axis xAxis = controller.AxisGet(Constants.AXIS_0_INDEX);
    Helpers.CheckErrors(xAxis);
    Axis yAxis = controller.AxisGet(Constants.AXIS_1_INDEX);
    Helpers.CheckErrors(yAxis);
    Axis zAxis = controller.AxisGet(Constants.AXIS_2_INDEX);
    Helpers.CheckErrors(zAxis);
 
    // configure phantom axes
    Helpers.PhantomAxisReset(xAxis);
    Helpers.PhantomAxisReset(yAxis);
    Helpers.PhantomAxisReset(zAxis);
 
    // set axis labels
    xAxis.UserLabelSet(xLabel);
    yAxis.UserLabelSet(yLabel);
    zAxis.UserLabelSet(zLabel);
 
    // create multi-axis object for joints
    MultiAxis jointsMultiAxis = controller.MultiAxisGet(0);
    Axis[] axes = [xAxis, yAxis, zAxis];
    jointsMultiAxis.AxesAdd(axes, axes.Length);
 
    // create simple 3-axis kinematic model
    const string modelName = "RSI_XYZ";
    const double scaling = 1.0;
    const double offset = 0.0;
    
    LinearModelBuilder builder = new(modelName);
    builder.UnitsSet(LinearUnits.Millimeters);
    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 });
 
    // create Robot object
    Robot robot = Robot.RobotCreate(controller, jointsMultiAxis, builder, MotionController.AxisFrameBufferSizeDefault);
    Helpers.CheckErrors(robot);
 
    // set path motion parameters
    robot.PathAccelerationSet(100);
    robot.PathVelocitySet(50);
    
    // program path motion
    robot.PathProgrammingModeSet(PathMode.Absolute);
    robot.PathLine(target: new Pose(1, 1, 1));
    robot.PathArc(
        target: new Pose(0, 2, 1),
        center: new Vector3d(0, 1, 1),
        direction: RotationDirection.Clockwise);
    
    // process the path to build trajectory
    robot.PathProcessLoadedMoves();
    
    // get path points for 3D rendering
    Console.WriteLine("Retrieving 3D path points for rendering:");
    const ulong frameCount = 500;
    ulong startFrame = 0;
    int totalPointCount = 0;
 
    ulong retrievedFrames;
    do
    {
        RapidVectorRobotPosition positions = robot.PathPlannedPositionsGet(startFrame, frameCount);
        retrievedFrames = positions.Size();
 
        foreach (RobotPosition position in positions.ToArray())
        {
            Vector3d spatial = position.Pose.Position;
 
            // print first 4 points as example
            if (totalPointCount < 4)
            {
                Console.WriteLine($"Point {totalPointCount}: X={spatial.X:F2}, Y={spatial.Y:F2}, Z={spatial.Z:F2}");
            }
 
            totalPointCount++;
 
            // add code here to create rendering with your preferred 3D library
            // examples:
            // vector3 = new Vector3D(spatial.X, spatial.Y, spatial.Z);
            // linePositions.Add(vector3);
            // scene.AddPoint(spatial.X, spatial.Y, spatial.Z);
        }
 
        startFrame += retrievedFrames;
    }
    while (retrievedFrames == frameCount);
 
    // print total points retrieved
    Console.WriteLine($"Total 3D points retrieved: {totalPointCount}");
 
    // cleanup
    Robot.RobotDelete(controller, robot);
}
// handle errors as needed
finally
{
    controller.Delete();  // dispose
}