FSU speed-limit compensation on palletizing robots

[ted-miller]

@EricMarcil @jimmy-mcelwain

We need to investigate to determine if this is actually an issue or not. I struggle to understand this code, and my brain is foggy right now.

The code which compares CMD to FB position uses mpGetPulsePos. But we also have Ros_CtrlGroup_GetPulsePosCmd, which compensates for this:

// For MPL80/100 robot type (SLUBT): Controller automatically moves the B-axis
// to maintain orientation as other axes are moved.

So would the FSU code have a problem because it doesn't perform the b axis compensation?

Also, I'm confused why Ros_CtrlGroup_GetFBPulsePos says:

//--------------------------------------------------------------------
//NOTE: Do NOT apply any B axis compensation here.
//      This is actual feedback which is reported to the state server.
//--------------------------------------------------------------------

[ted-miller]

Those comments came from here: ros-industrial/motoman#191
and here: ros-industrial/motoman@c367f27

There were both in response to a customer request through YEC.

[EricMarcil]

Those robot used to have back links that would mechnically keep the B-axis parallel to the ground. This was eventually replaced by keeping the B-axis motor, but a B-axis compensation was introduced to keep the B-axis in the same position relative to the ground when moving the L and U axes. For example, if you look at this start position:

And then jog the L-axis:

You can see that the B-axis was also changes even if only the L-axis was pressed.

This automatic compensation is also applied when moving the robot using the MotoPlus. But ROS command would be where the user wants the robot to be and wouldn<t be aware of the compensation that the controller will add. So when we receive the command from ROS, we remove the equivalent of the compensation that the controller will add in, so they cancel out and the robot ends up at the ROS commanded position.

So the command position that is with the removed compensation, so if we want to know where exactly the robot will end-up with pulse command position and compare it to the ROS position, we have to add the compensation to the command.

[ted-miller]

I think I understand now. It looks like we do NOT have an issue with the FSU code.

[EricMarcil]

But looking at the Ros_MotionServer_InitTrajPointFull there might be an error. We are removing the compensation from the ROS start point and converting it to the MotoPos 'pulsePos' in the code here:

// For MPL80/100 robot type (SLUBT): Controller automatically moves the B-axis
// to maintain orientation as other axes are moved.
if (ctrlGroup->bIsBaxisSlave)
{
	//ROS joint order
	ctrlGroup->jointMotionData.pos[3] += -ctrlGroup->jointMotionData.pos[1] + ctrlGroup->jointMotionData.pos[2];
	ctrlGroup->jointMotionData.vel[3] += -ctrlGroup->jointMotionData.vel[1] + ctrlGroup->jointMotionData.vel[2];
}

// Convert start position to pulse format
Ros_CtrlGroup_ConvertToMotoPos(ctrlGroup, ctrlGroup->jointMotionData.pos, pulsePos);

But right after we are using the Ros_CtrlGroup_GetPulsePosCmd(ctrlGroup, curPos) which added the compensation back in to get the 'curPos'

Ros_CtrlGroup_GetPulsePosCmd(ctrlGroup, curPos);

So, 'pulsePos' is without compensation and 'curPos' is with compensation and then we do:

// Check if position matches current command position
if(abs(pulsePos[i] - curPos[i]) > START_MAX_PULSE_DEVIATION)

So, that will not evaluate properly the START_MAX_PULSE_DEVIATION when starting a new trajectory while the robot is still finishing off the previous one.

The Ros_CtrlGroup_GetPulsePosCmd is also used in several places. So it should probably be reviewed thoroughly.

A side note: Ros_MotionServer_InitTrajPointFull
Ros_CtrlGroup_GetPulsePosCmd is called twice in a row to set curPos and ctrlGroup->prevPulsePos. I would be more efficient to copy the curPos to ctrlGroup->prevPulsePos directly, rather than executing the Ros_CtrlGroup_GetPulsePosCmd a second time.

[ted-miller]

'pulsePos' is without compensation and 'curPos' is with compensation and then we do

I don't think that's true. They both have the compensation applied.

Plus, I'm confident that this scenario was working correctly in the MotoROS1 code you're referencing. This was developed for a specific customer. My email chains indicate that they were satisfied with this specific issue.

The Ros_CtrlGroup_GetPulsePosCmd is also used in several places. So it should probably be reviewed thoroughly.

I definitely agree with that. There's a very good chance that I've been using that without thinking.