Yaskawa-Global/motoros2

Compatibility with "Absolute Accuracy Compensation"

yeu-geissdoerfer opened this issue · 20 comments

yeu-geissdoerfer commented

We had a case where we set the Absolute Accuracy Compensation to valid on a YRC1000, using controller SW version YAS 5.20.00A-00. Once we had taken all the necessary steps to enable the Absolute Accuracy Compensation Function, we were able to connect the microROS agent with the MotoROS2 driver.

To test the accuracy of the motion, we used the last published position of the JointState.msg as the start position of our trajectory and defined a desired joint state as a goal. After the trajectory was sent to the driver, it immediately rejected it and sent the following Error Message

[2024-01-29 14:47:07.192195] [192.168.0.2:57915]: Ros_ServiceStartTrajMode_Trigger: activated
[2024-01-29 14:47:12.451865] [192.168.0.2:57915]: Trajectory contains 2 points
[2024-01-29 14:47:12.452465] [192.168.0.2:57915]: Initializing trajectory for group #0
[2024-01-29 14:47:12.452665] [192.168.0.2:57915]: ERROR: Trajectory start position doesn't match current position (MOTO joint order).
[2024-01-29 14:47:12.452665] [192.168.0.2:57915]:  - Requested start: -264, -1030, 121, -72, -135, -63, 0, 0
[2024-01-29 14:47:12.452665] [192.168.0.2:57915]:  - Current pos: 0, 0, 0, 1, 0, 1, 0, 0
[2024-01-29 14:47:12.452665] [192.168.0.2:57915]:  - ctrlGroup->prevPulsePos: 0, 0, 0, 1, 0, 1, 0, 0
[2024-01-29 14:47:12.452865] [192.168.0.2:57915]: FollowJointTrajectory - Goal request rejected
[2024-01-29 14:47:12.452865] [192.168.0.2:57915]: The trajectory will be accepted and then immediately aborted
[2024-01-29 14:47:12.453065] [192.168.0.2:57915]: The first point must match the robot's current position.

The failure is due to the fact, that the first point doesn't match the robot's current position which is checked in

motoros2/src/MotionControl.c

Lines 194 to 212 in b71ced5

// Check for each axis
for (int i = 0; i < MAX_PULSE_AXES; i++)
{
// Check if position matches current command position
if (abs(pulsePos[i] - curPos[i]) > START_MAX_PULSE_DEVIATION)
{
Ros_Debug_BroadcastMsg("ERROR: Trajectory start position doesn't match current position (MOTO joint order).");
Ros_Debug_BroadcastMsg(" - Requested start: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld",
pulsePos[0], pulsePos[1], pulsePos[2],
pulsePos[3], pulsePos[4], pulsePos[5],
pulsePos[6], pulsePos[7]);
Ros_Debug_BroadcastMsg(" - Current pos: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld",
curPos[0], curPos[1], curPos[2],
curPos[3], curPos[4], curPos[5],
curPos[6], curPos[7]);
Ros_Debug_BroadcastMsg(" - ctrlGroup->prevPulsePos: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld",
ctrlGroup->prevPulsePos[0], ctrlGroup->prevPulsePos[1], ctrlGroup->prevPulsePos[2],
ctrlGroup->prevPulsePos[3], ctrlGroup->prevPulsePos[4], ctrlGroup->prevPulsePos[5],
ctrlGroup->prevPulsePos[6], ctrlGroup->prevPulsePos[7]);

Looking deeper into the code shows that there are two different MotoPlus functions used to determine the robots position

  • pulsePos uses mpGetFBPulsePose() (FB stands for feedback) -> current actual position

    motoros2/src/CtrlGroup.c

    Lines 337 to 338 in b71ced5

    // get raw (uncorrected/unscaled) joint positions
    LONG status = mpGetFBPulsePos (&sData,&pulse_data);
  • curPos uses mpGetPulsePose() -> current commanded position

    motoros2/src/MotionControl.c

    Line 189 in b71ced5

    Ros_CtrlGroup_GetPulsePosCmd(ctrlGroup, curPos);

When we look into the "YRC1000 Options Instructions for Absolute Accuracy Compensation Function" the following is stated there

Absolute Accuracy Compensation Function

The absolute accuracy compensation function positions the TCP with high accuracy to a position specified in Cartesian coordinates.

Restrictions

When absolute accuracy compensation function is set to valid, only the reference value is available in the robot current position output function. The feedback value cannot be used!

Due to the Absolute Accuracy function, the actual axis position is displayed via the feedback value mpGetFBPulsePose(). However, this differs from the command position mpGetPulsePose().

The functions Ros_PositionMonitor_UpdateLocation() and Ros_Controller_IsInMotion() are also affected, since they also use mpGetFBPulsePose().

Question

Which MotoPlus function can be used instead, since mpGetFBPulsePose() has a trailing error?

gavanderhoorn commented

I'm going to mention #199 just to keep things linked, as it mentions the same/similar functions.

ted-miller commented

I wasn't aware that function was available on the YRC.

My first inclination is to say that this function should not be used with MotoROS2.

  • Detection of the difference between cmd and fb is the foundation for detecting FSU interference.
  • Users have had issues in the past with detecting that a trajectory has finished. This is why Ros_Controller_IsInMotion is used.
  • Based on a quick skim through the documentation, it appears this function is just a high-accuracy calibration method for cartesian data. But MR2 doesn't use the YRC's kinematic solver. So, I expect that this calibration data would be ignored.
gavanderhoorn commented

@ted-miller: while there doesn't appear to be a direct advantage to using absolute accuracy together with MotoROS2, as we indeed currently don't have any Cartesian interfaces, nor do we use Cartesian motion primitives while executing motion, could we perhaps find a way to make MotoROS2 compatible with it?

As mentioned, we can't just wholesale switch to mpGetPulsePose(..), as that would break functionality in MotoROS2 elsewhere.

@yeu-geissdoerfer: are you mostly interested in just being able to use MotoROS2 on a YRC1000 with Absolute Accuracy enabled -- but not necessarily absolute accurate motions, or do you really want/need to also have absolute accurate motions with MotoROS2?

The former could potentially be possible. The latter probably wouldn't be.

yeu-geissdoerfer commented

I would be interested in absolute accurate motions with MotoROS2 as this seems to be particularly important for the HC series.

gavanderhoorn commented

I would be interested in absolute accurate motions with MotoROS2

Unless I've misunderstood, the absolute accurate motions will only improve Cartesian motions. MotoROS2 does not support those.

as this seems to be particularly important for the HC series.

Your OP mentions a YRC1000 controller. Should that be a 1000 micro then?

yeu-geissdoerfer commented

It is a YRC1000 controller, no micro.

gavanderhoorn commented

@yeu-geissdoerfer: it's clear it would be good for MotoROS2 to be able to at least still move a robot with abs acc compensation enabled. But that would be something different from being able to use abs acc compensation with motions controlled by MotoROS2.

From the documentation (198323-1CD (HW2481134), Section 1.1):

The absolute accuracy compensation function positions the TCP with high accuracy to a position specified in Cartesian coordinates

From chapter 5:

Temporarily Invalidate Compensation

This chapter describes the method to temporarily invalidate compensation by the absolute accuracy compensation function. Use this function in cases such as those in the following examples.

  • To teach the actual robot by using the compensation invalid move instruction (absolute pulse move instruction) (refer to chapter 6 "Compensation Invalid Move Instruction (Absolute Pulse Move Instruction)" )

Chapter 6 then continues to explain when abs acc should be disabled. It also specifically mentions pulse based motions.

yeu-geissdoerfer commented

@gavanderhoorn what would be a scenario for you in which it is necessary to move the robot with MotoROS2 and Abs Acc Compensation activated?

gavanderhoorn commented

I can think of two:

  1. a hybrid application where MotoROS2 is used for non-process motions (fi: to get an EEF into position using collision free motion planning) and then starting an INFORM job to perform process motion (fi: welding, blending, etc)
  2. a robot which is used both with ROS and with regular INFORM jobs, but not at the same time

basically it would include all motions performed by MotoROS2 which do not make use of / can't benefit from abs acc.

Which would be all of its motion, IIUC. We're only using pulse based joint motion at this time.

Edit: note I'm not claiming there is no value in abs acc per se. It would be good to find a way for it to be used with MotoROS2. I just wanted to make sure we are all on the same page.

  • 👍1
gavanderhoorn commented

Created #207 to track the need to document compatibility with abs acc compensation.

gavanderhoorn commented

@yeu-geissdoerfer: just making sure: would you still like to see a work-around / solution to the issue you raised, or have the previous comments clarified things sufficiently?

yeu-geissdoerfer commented

I have the following question:

If our robots want to use Absolute Accuracy Compensation, they must first go through a special measurement procedure. This determines the actual DH parameters.

  1. Could these actual DH parameters be used to update the existing URDF file of this robot, to compensate the geometrical error while planning the motion?
  2. How could such a URDF file be updated automatically?

It would then be possible to compensate the geometric error without enabling the Absolute Accuracy Compensation on the robot controller.

gavanderhoorn commented

While I'm not sure whether this isn't actually an X-Y problem: yes, that could very likely be done.

See the way ur_calibration does this, by converting their D-H parameters to a format compatible with URDF frames. These values can be stored in a .yaml file and then loaded at runtime to update a 'template' URDF. See ros-industrial/universal_robot#357 for the original discussion.

A similar approach is used by some users of MotoROS1 (and motoman_driver), although they/we haven't upstreamed that.

Edit: a discussion on the MotoROS1 Discussion board about this: ros-industrial/motoman#459.

iydv commented

According to this comment ros-industrial/motoman#459 (comment) UR actually extracts 6D parameters of each joint instead of D-H parameters. And use it to directly generate URDF model instead of correcting the default one.

Which information is currently possible to extract from the robot using MotoSDK? Is it possible to extract information of calibrated kinematics from Absolute Accuracy?
Is it possible to extract D-H parameters? If true, where information can be found about its definition for Yaskawa robots (similar to UR).

Main reason for this request is follows:

To improve precision of Yaskawa robots in vision application users can either rely on external calibration solutions or use new Absolute Accuracy feature from Yaskawa directly. If MotoROS2 is incompatible with Absolute Accuracy, ROS2 users can at least extract calibration information and update default URDF model similar to what UR offers. That alone can already improve precision of Cartesian motions.

ted-miller commented

Which information is currently possible to extract from the robot using MotoSDK

We do allow for extracting DH parameters from the robot.

motoros2/src/CmosParameterExtraction.h

Line 205 in 10d8f00

extern STATUS GP_getDhParameters(int ctrlGrp, DH_PARAMETERS* dh);

But the DH parameters for the robot never get updated after a calibration. Normally, we only modify the encoder's zero-position when calibrating.

Is it possible to extract information of calibrated kinematics from Absolute Accuracy

I don't know enough about this function yet. It is very new.

We'll need to investigate more.

gavanderhoorn commented

Could I suggest we discuss calibrated URDFs and DH parameters in #210?

It's all related to Abs Acc Compensation, but at the same time we're starting to go off-topic.

gavanderhoorn commented

@yeu-geissdoerfer: are you assisting @iydv with their request to make motions more accurate, and is that the context in which the question about abs acc compensation came up?

(I ask because if the real desire is to make motion more accurate, calibrated URDFs would be something most likely more compatible with MotoROS2 than abs acc compensation, seeing as MotoROS2 doesn't use any Cartesian motion).

yeu-geissdoerfer commented

@gavanderhoorn: Yes, I support @iydv in his request to make motions more accurate, and that's how my question about abs acc compensation came up.

(We are now investigating the calibrated URDFs as this seems to be the most promising solution at the moment.)

gavanderhoorn commented

We are now investigating the calibrated URDFs as this seems to be the most promising solution at the moment

could we discuss approaches in #210? I don't believe there are any standard or best practices at the moment, and this seems like a good opportunity to design something for Yaskawa support packages.

  • 👍1
gavanderhoorn commented

@yeu-geissdoerfer: would you have any update wrt Abs Acc compatibility?

With #207 we basically added a section explaining there is currently no compatibility, but I'd be interested to learn what you've discovered so far.