path-3d-points.cs

/*  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.ConfigurePhantomAxis(xAxis);
    Helpers.ConfigurePhantomAxis(yAxis);
    Helpers.ConfigurePhantomAxis(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
}