MultiAxis PT streaming motion

[rdhillon]

In order to use PT motion, do I have to call this function repeatedly, in a loop perhaps?

axis.MovePT(RSIMotionType.RSIMotionTypePT, first, time1, points, emptyCount, false, false);

For a 2 axis system, Axis X and Axis Y which one will applly?

Option 1
axisX.MovePT(RSIMotionType.RSIMotionTypePT, firstX, time1, points, emptyCount, false, false);
axisY.MovePT(RSIMotionType.RSIMotionTypePT, firstY, time1, points, emptyCount, false, false);

Or Option 2: I am combining the positions

// Create a combined positions array
std::vector positions;

for (int i = 0; i < TOTAL_POINTS; ++i) {
    positions.push_back(positionsX[i]);  // Axis X position
    std::cout << "Positions X: " << positionsX[i] << std::endl;
    positions.push_back(positionsY[i]);  // Axis Y position
    std::cout << "Positions Y: " << positionsY[i] << std::endl;
}

for (int i = 0; i < TOTAL_POINTS; ++i) {
multiAxis->MovePT(
RSIMotionType::RSIMotionTypeBSPLINE, // Motion type
&positions[i], // Pointer to the first element of combined positions
&times[i], // Pointer to the first element of times
TOTAL_POINTS, // Number of time slices (common for both axes)
EMPTY_CT, // E-stop empty count threshold
false, // Whether points are kept or not
true // Specify if this is the last MovePT
);
}

[todd_mm]

MovePT(...) takes a flattened, 2-D array of waypoints:

[ x_0, y_0,
  x_1, y_1,
  x_2, y_2,
  ...,
  x_n, y_n ]

The array should have TOTAL_POINTS × multiAxis->AxisCountGet() elements in it.

The sample code doesn’t really illustrate this very well, though the function description does better.

*position Array of positions (p) to move through. (positions are in the UserUnits for each Axis)
Axis: [p0, p1, p2, …, pn]

[jacob]

Hi @rdhillon,
Either method would work, but I’d almost certainly use the 2nd if the two streams of motion are supposed to be coordinated. Synchronizing the two with the first method could be done with a motion hold but that seems needless complicated.

[rdhillon]

Thank you Todd.

Quick question. Does x_0, y_0 ( or the first element pairs) for the array represent the starting point or first end points. If I am starting from 0, then should I write 0,0 as the first elements. Its a 2 axis system.

[todd_mm]

When streaming, you’re sending a series of waypoints to RMP to execute.
The first x,y pair you send is the first point to pass through. It should probably be the current position of the motors, though that’s not a strict requirement.

[rdhillon]

Continuing on the discussion. I think I am doing it right. I have two axis so number of elements in positions array is twice the number of elements in time array. I have generated 3 arrays. arrayAxisX, arrayAxisY and arrayTime. Each has 100 values.

I am then interleaving the axisX and axisY elements so I have a flat array as suggested in the documentation.

Here is the code snippet I am using to test the theory.

const int TOTAL_POINTS = 100;
const int POINTS_PER_CALL = 10;
const int AXIS_COUNT = 2;  // 2 axes: X and Y

std::vector<double> arrayAxisX(TOTAL_POINTS);  // Simulating 100 X-axis positions
std::vector<double> arrayAxisY(TOTAL_POINTS);  // Simulating 100 Y-axis positions
std::vector<double> arrayTime(TOTAL_POINTS);   // Simulating 100 time points

// Filling the arrays with some data (for illustration purposes)
for (int i = 0; i < TOTAL_POINTS; i++) {
    arrayAxisX[i] = i * 0.01;  // Example X-axis values
    arrayAxisY[i] = i * 1;  // Example Y-axis values
    arrayTime[i] = i * 0.01;   // Example time values
}

// Iterate over the total points in chunks of POINTS_PER_CALL
for (int i = 0; i < 100; i += POINTS_PER_CALL) 
 {
   // Interleave the position data for both axes
   std::vector<double> positions;
   for (int j = 0; j < POINTS_PER_CALL; j++) {
        positions.push_back(arrayAxisX[i + j]);
        positions.push_back(arrayAxisY[i + j]);
   }

    // Pointer to the time data for this chunk
    const double* time = &arrayTime[i];

    // Determine if this is the final motion call
    bool final = (i + POINTS_PER_CALL >= 100);

    // Visualization: Print the position and time data
    std::cout << "Iteration " << i / POINTS_PER_CALL + 1 << ":\n";
    std::cout << "Positions: [ ";
    for (double p : positions) {
        std::cout << p << " ";
    }
    std::cout << "]\n" << "Positions Array Size: " << positions.size() << "\n" ;

    std::cout << "Time: [ ";
    for (int j = 0; j < POINTS_PER_CALL; j++) {
        std::cout << time[j] << " ";
    }
    std::cout << "]\n" ;

    std::cout << "Final: " << std::boolalpha << final << "\n\n";

    // Execute motion with multiAxis->MovePT
    multiAxis->MovePT(RSIMotionType::RSIMotionTypePT, &positions[0], time, POINTS_PER_CALL, 20, true, final);
    multiAxis->MotionDoneWait();
}

Here is the screenshot of what I am getting in the output. Its not iterating beyond 1 and giving this error. Not sure why its saying non positive time delta in points[index].

I am trying to pass the points in chunks of 10 elements in this example until I have exhausted all 100 points when the “final” parameter becomes true and I exit out of loop.

[scott]

The error message,

Parameter invalid :: {motion.c, line 13082} : Non-positive timeDelta in points[index] : index = 0 : timeDelta = 0 (Error 2) (RapidCodeMotion::MovePT_PVT) (Object 2) (File rapidcodemotion.cpp) (Line 1291) (Version 10.5.5.0)

notes that you have a non-positive time delta.

per the MovePT documentation:

*time	Array of time durations for each point in seconds.
Minimum value is your MotionController sample period. Values must be a multiple of the sample period.
If each point should take one millisecond, your array would be: [0.001, 0.001, 0.001, etc.]

You cannot have a value of 0 for the time. In your case, I think you just want each element to be 0.001. (Not incrementing).

Since you’re working with vectors, you can also can just pass in positions.data() like this:

multiAxis->MovePT(RSIMotionType::RSIMotionTypePT, positions.data(), etc

[rdhillon]

Thank you Scott.

I changed my arrays to have fixed increments of time for time being.

// Filling the arrays with some data (for illustration purposes)
for (int i = 0; i < TOTAL_POINTS; i++) {
arrayAxisX[i] = i * 0.01; // Example X-axis values
arrayAxisY[i] = i * 1; // Example Y-axis values
arrayTime[i] = .01; // Example time values
}

The non positive time delta error is gone but there is a new one now.

the loop iterates 2 times and then it throws the following error.

Use hardware is set to false.

[rdhillon]

Hi all again. reviving this thread again as I have made some progress of integrating the motion how I want. However, I have run into some issues.

I am getting MPI State error when I run PT motion. I am attaching some data.

In the data I am converting x, y coordinates to user units represented by “Motor Array” based on kinematics of my ssytem. Time delta is a constant value of 0.1.

When I execute this the motor goes to 0 position and then makes a very small motion and then I get this error.

This the function I am calling when executing the motion.

void PTMotion(MotionController * controler, int AXIS_COUNT, MultiAxis * axis, const std::vector &posX, const std::vector &posY, const std::vector &timeDelta)
{
std::vector positions;
// Interleave the position data for both axes into positions array
for (int i = 0; i < timeDelta.size(); i++)
{
positions.push_back(posX[i]);
positions.push_back(posY[i]);
}

std::vector initXY, initTime;
initXY = {0, 0, 0, 0 };
initTime = {1, 1};

// reset the motion ID to 0
axis->MovePT(RSIMotionType::RSIMotionTypeSPLINE, initXY.data(), initTime.data(), 1, -1, false, true);
axis->MotionIdSet(0);
int myTimeoutMS = 5000;
axis->MotionDoneWait(myTimeoutMS);

axis->MovePT(RSIMotionType::RSIMotionTypeSPLINE, &positions[0] , &timeDelta[0] , timeDelta.size()/10, 5, false, true);
axis->MotionDoneWait(myTimeoutMS *2);

}

Is there anything I am doing wrong?

TIA

[rdhillon]

Yeah. I also have fixed values. those are represented by Time Delta in my picture. I am not displaying it under “Motor Array” . But my values are set up at 0.1 instead of 0.001.

[jacob]

Hi @rdhillon,

Typically an StateERROR is due to not readying the axis for the motion command. I think you can resolve this by just using ClearFaults first. More generally you want to make sure you are ready for the move. Evaluate if you are still moving and determine if you want to wait, estop, etc before clearing faults.

Consider the following code but customize it to your needs:

void ReadyAxis(Axis axis)
{
  const int secondsToMilliseconds = 1000;

  switch (axis->StateGet())
  {
    //Assuming a simple Abort is safe here.
    case RSIState::RSIStateMOVING:
      axis->Abort();
      axis->ClearFaults();
      break;
    //Wait standard stop time
    case RSIState::RSIStateSTOPPING:
    case RSIState::RSIStateSTOPPING_ERROR:
      //Assuming that Stop Time is greater than EStop Time.
      std::this_thread::sleep_for((axis->StopTimeGet() * secondsToMilliseconds));

      //You may still be stopping if you have bad settings.
      if (axis->StateGet() == RSIState::RSIStateSTOPPING ||
         axis->StateGet() == RSIState::RSIStateSTOPPING_ERROR)
      {
        //Abort the axis.  You should stopped but likely have bad settling criteria.
        axis->Abort();
      }
      axis->ClearFaults();
      break;
    case RSIState::RSIStateERROR:
    case RSIState::RSIStateSTOPPED:
      axis->ClearFaults();
      break;
    case RSIState::RSIStateIDLE:
    default:
      //No action needed
      break;
  }

  const int MAX_ENABLE_TIME = 1000; //Selected in general, you can specific something appropriate to your hardware.
  axis->AmpEnableSet(true, MAX_ENABLE_TIME);

  if (!axis->AmpEnableGet())
  {
    throw new Exception("Failed to Ready the Axis.  State: " + axis->StateGet());
  }
}

This is just an example. I don’t have any safety concerns with aborting an axis but any specific application might.

[jacob]

Ophs I didn’t include MAX_ENABLE_TIME as an overloaded parameter to AmpEnableSet. That should have been included. Code updated.

[rdhillon]

I tried the modified version of the code and noticed a thing.

I have about 21 elements in time delta array. And I have 42 elements in positions array because its a 2 axis move.
I am using BSPLINE method PTMove. When I send all the data in one go, the motors executes the motion with no issues.

However, If I divide the data transmission and send the arrays in chunks, I get RSI state error.

In this particular example, I am dividing the array into 3 chunks. Sending 7 time elements in the first try, then 7 more and finally whatever is left in the array. the last transmission is also toggling the final to TRUE as required in the MovePT function.

[jacob]

Hi @rdhilllon,

At the low level it is just checking the state of the axis when validating that the passed values are valid. What is happening between the two iterations? Is it possible that you’ve passed the 2.8 seconds to process through the initial 14 points? Can you throw up a timestamp and check the state of the multiaxis object with each Iteration print out?

[rdhillon]

Nothing is happening between iterations. I am just testing methods where I can send data in chunks so the buffer does not overflow for movePT function and for handeling interrupts. I took inspiration from the sample program called UpdateBufferPoints.cpp but that is also not working as expected. So I created this program in which I am sending data in chunks as its done in sample code but without interrupts.

This is the function block executing this motion.

void PTMotion(MotionController * controller, int AXIS_COUNT, MultiAxis * multiAxis, const std::vector &posX, const std::vector &posY, const std::vector &timeDelta)
{

const int TOTAL_POINTS = timeDelta.size();
const int POINTS_PER_CALL = (timeDelta.size()/3);
const int EMPTY_COUNT = ceil((POINTS_PER_CALL * AXIS_COUNT));

std::vector positions;

// Iterate over the total points in chunks of POINTS_PER_CALL
for (int i = 0; i < TOTAL_POINTS; i += POINTS_PER_CALL)
{

int pointsThisIteration = std::min(POINTS_PER_CALL, TOTAL_POINTS - i);
// Interleave the position data for both axes
std::vector<double> positions;
for (int j = 0; j < pointsThisIteration; j++)
{
  positions.push_back(posX[i + j]);
  positions.push_back(posY[i + j]);
}

// Pointer to the time data for this chunk
const double *time = &timeDelta[i];

// Determine if this is the final motion call
bool final = (i + pointsThisIteration >= TOTAL_POINTS);

// Visualization: Print the position and time data
std::cout << "Iteration " << i / POINTS_PER_CALL + 1 << ":\n";
std::cout << "Empty Count " << EMPTY_COUNT << ":\n";
std::cout << "Processing " << pointsThisIteration << " points:\n";
std::cout << "Positions: [ ";
for (double p : positions)
{
  std::cout << p << " ";
}
std::cout << "]\n"
          << "Positions Array Size: " << positions.size() << "\n";

std::cout << "Time: [ ";
for (int j = 0; j < pointsThisIteration; j++)
{
  std::cout << time[j] << " ";
}
std::cout << "]\n";

std::cout << "Final: " << std::boolalpha << final << "\n\n";

// Execute motion with multiAxis->MovePT
multiAxis->MovePT(RSIMotionType::RSIMotionTypeBSPLINE, &positions[0], time, pointsThisIteration, EMPTY_COUNT, false, final);
multiAxis->MotionDoneWait();

}
//multiAxis->MotionDoneWait();

}

Its in Ideal or Default state before the motion starts.

[rdhillon]

When I execute the same code with just one iteration. It works as expected.

In my function block I am just dividing the POINTS_PER_CALL by 1 instead of 3.

[jacob]

Hi @rdhillon,

It looks like you are erroring out on the 2nd call rather than the 3rd since we don’t see any printing for the final point. My first thought is you might have a positioning error limit active if the current position > position error limit. If your starting position isn’t 0, you would command a move to 0 with a profile that might put you in an error state.

Can you print out your input arrays to take a close close?
Can you print out your actual position of each axis at the time of entering the function?
(Alternatively, you could try a PositionSet(posX[0]) and PositionSet(posY[0])

[rdhillon]

These are the outputs of startup.

I removed the MotionDoneWait from my function but it did not help.

[jacob]

Hey @rdhillon,

Can you put a timestamp on each iteration for evaluation? Move the State check within the iteration. I expect it to report Error for the 2nd pass.

[rdhillon]

Yes. Here is the output snapshot. You are correct. Its failing in the 2nd iteration. but the Motor has not moved at all for the points in the 1st iteration