Robot doesn't reach commanded joint state

[riv-dpowell]

We are trying to debug some repeatability issues with our GP25 which is controlled with the JointTrajectoryAction in a multi group setup.

Commanding a waypoint on the pendant ends with the robot at exactly the same joint state as commanded to the identical pulse level.

Using the JointTrajectoryAction commanding a trajectory ending at:
[0.0007928455826146039, 0.08002201343656831, 0.02247886556465276, 0.008098200800699196, 0.1008953169889391, -0.008119412882377676]

Finishes with the robot at:
[0.0007806866778992116, 0.0800170749425888, 0.022472817450761795, 0.008343899622559547, 0.10088873654603958, -0.00809883326292038]

So giving errors of:
[1.21589047153923E-05, 4.93849397950608E-06, 6.04811389096419E-06, -0.000245698821860351, 6.58044289951476E-06 , -2.0579619457296E-05]

Which in pulses is:
[0.934479981806983, 0.539787365780375, 0.551986843865635, -14.3993990079501, 0.371781649087494, -0.632725840339016]

I've tried a few different poses and the error always seems to be the same magnitude on the 4th joint.

We've tried chainging the goal tolerance but were having trajectories time out with a tolerance less than 0.001, and 0.001 didn't make a difference to the error. (See #591) for my issue related to setting that parameter, I worked around by changing the default value.

Is there anything which could be misconfigured somewhere or anywhere obvious to look for this discrepancy?

[ted-miller]

Some of that error can be explained by rounding errors when converting between radian and pulse.

However, I'm also suspicious of the logic which determines when a motion is complete. I think it may be triggered as "done" a little too early. See Yaskawa-Global/motoros2#254.

I'd be curious to know if that error amount tightens up after a second or so. If you look at the current position a little after the trajectory finishes, has the error deviation gotten any smaller?

[riv-dpowell]

Thanks for the reply.

Some of that error can be explained by rounding errors when converting between radian and pulse.

I think that explains the errors in all the joints except 4.

I'd be curious to know if that error amount tightens up after a second or so. If you look at the current position a little after the trajectory finishes, has the error deviation gotten any smaller?

The measured joints are taken after a 0.5 second wait. I can try increasing that but my gut feeling is that it won't make a difference.

I've been doing some more testing and have a plot that clearly shows the problem, strangely it doesn't happen for every single pose. There's nothing systematic we can see between the poses where it does and doesn't happen.

Below is a plot of our measurements, this was created by commanding the same 8 poses 10 times, then recording the commanded and reached joints.

[riv-dpowell]

We also tried a similar test using our GP4 with the standard single group JointTrajectoryAction and didn't see the same problem.

[ted-miller]

I think the first step is to identify whether the issue exists in MotoROS or on the PC. If you could get a wireshark capture of the data between the PC and robot, then we can determine if the robot is reaching the position that it's commanded.

[riv-dpowell]

Looking at the commanded joint values. The error seems to always happen when joint_4 < 0 and never when joint_4 > 0. I'm going to collect a much wider range of poses. I'll also look into the wireshark capture.

[riv-dpowell]

I've done some more testing. Attached is a spreadsheet with the error in pulses between the targeted and reached pose for 60 joint states.

60_pose_errors.csv

It seems there are 3 error categories:

• ~15 pulse error when R < 0
• All joints bad (probably some kind of random error)
• Another joint is bad (uncorrelated to target joints)

---

I've used Wireshark to inspect the communications between the robot and PC. Attached is an XML file containing the relevant sections: wireshark_joint_error.zip

My analysis is:

The final target pose sent as a ROS-Industrial SimpleMessage, Joint Trajectory Point Full message is:

[-0.081698865, -0.591372490, -0.567616701, -0.565098763, 0.501540899, 0.207804993]

In the first Motoman Joint Feedback Extended message after a status message where inMotion is false, position is:
[-0.081698865, -0.591409087, -0.567605734, -0.565337598, 0.501594007, 0.207512274]
error in pulses:
[0, -4.00012601158515, 1.00091364444589, -13.9971385944209, 3.00049405811324, -8.99972303387433]

The first Motoman Joint Feedback Extended message where all velocities are 0 has position:
[-0.081698865, -0.591372490, -0.567605734, -0.565354705, 0.501523197, 0.207804993]
error in pulses:
[0, 0, 1.00091364444589. -14.9997096159768, -1.00012702072296, 0]

A Motoman Joint Feedback Extended message 1s after inMotion is false has position:
[-0.081698865, -0.591372490, -0.567605734, -0.565354705, 0.501523197, 0.207804993]
error in pulses:
[0, 0, 1.00091364444589, -14.9997096159768, -1.00012702072296, 0]

[riv-dpowell]

Further investigation:

• I've tried reducing the trajectory speed and disabling FSU zones (we aren't using FSU speed limits) and this had no effect
• I've done some digging into the MotoROS codebase, my only thought was whether pulse correction could be causing this issue?

  motoman/motoman_driver/MotoPlus/ParameterExtraction.h

  Line 81 in 433be15

  extern STATUS GP_getFBPulseCorrection(int ctrlGrp, FB_PULSE_CORRECTION_DATA *correctionData);

[ted-miller]

Hi @rr-dave
I've been looking at the numbers and I think that the issue is just that MotoROS is declaring motion "complete" too early. (Yaskawa-Global/motoros2#254)

The repeatability spec of a GP25 is 0.02 mm.

In your example above, the positions work out to:

	x (mm)	y (mm)	z (mm)
cmd	1145.771	-67.985	-81.652
fb	1145.773	-67.976	-81.649
diff	-0.002	-0.009	-0.003

---

Commanding a waypoint on the pendant ends with the robot at exactly the same joint state as commanded to the identical pulse level.

The CURRENT POSITION button on the pendant does not display true feedback position.

Please try looking at the SERVO MONITOR screen. Above this screen, touch DISPLAY > MONITOR ITEM 1 > FEEDBACK PULSE. This shows the true feedback. There is also a display for ERROR PULSE that you can monitor.

---

I've done some digging into the MotoROS codebase, my only thought was whether pulse correction could be causing this issue?

The pulse correction is applicable to your robot. Additionally, it could contribute to more rounding errors in the application.

On many robot models (including GP25), the motors for the T and/or B axes are physically mounted behind another axis. Moving one axis (such as B) causes the encoder for another axis (such as T) to move. The software compensates for this and shows you a "logical" pulse position. But it will not match what is displayed on the FEEDBACK screen.

[riv-dpowell]

Hi Ted, thanks for the reply I've made some progress.

By looking at the servo monitor screen I can see the same 15 pulse error when running an INFORM job on the pendant. By doing some jogging on the pendant I think the error isn't caused by a negative R joint value, but moving in the negative R joint direction which smells like some kind of backlash correction.

We originally noticed this discrepancy when we were investigating a "Trajectory start position doesn't match current robot position" error. Is it deliberate that the commanded position is in the "corrected joint space" whereas the joint feedback messages are in the "raw servo space"?

[ted-miller]

Yes, it is by design that "current position" is the true feedback position. When calibrating the robot to external sensors, it's important to have actual position of the arm.