Modifying the interpolation loop inside MotoPlus

[yueminm]

I'm trying to execute an offline dense trajectory on a GP12 with YRC1000 controller. I can generate the trajectory such that its timestep matches the interpolation period, which means I don't need any extra interpolation steps from MotoPlus.

I'm wondering if modifying the interpolation loop in MotionServer.c is the right approach. If so, do I need to use MotoPlus SDK to recompile the code? If not, do you have any suggestions on how to do that?

Thank you so much!

[ted-miller]

Hi @yueminm
Such an approach has been done before. https://github.com/tingelst/motoman/blob/e584f3e8e4d29e6882cd9160a1593b6ead5713cd/motoman_driver/MotoPlus/RealTimeMotionServer.c#L402
I'm not sure how successful this was.

Personally, I have a few concerns with it:

• There's the potential for packets to arrive out of order
• This calls a bunch of API's that are not deterministic. It is very likely that interpolation cycles will be missed.

do I need to use MotoPlus SDK to recompile the code?

Yes.

[gavanderhoorn]

What if the JointTrajectory already has a time-resolution of 4ms?

I guess that's what @yueminm is asking: if the trajectory submitted as part of the JointTrajectoryAction goal is already sufficiently dense, resampling by MotoROS should not be necessary.

The code in Ros_MotionServer_JointTrajDataToIncQueue(..) would have to 'recognise' that though. It might actually already do the right thing, just because it looks at time_from_start. It does always recalculate acceleration though.

The ROS side doesn't do any preprocessing other than converting the ROS JointTrajectory into its SimpleMessage representation.

[yueminm]

Thank you so much for your quick reply!

Yes this is what I was asking about and Ros_MotionServer_JointTrajDataToIncQueue(..) was the part of code I tried to modify.

In more details:
I would like to execute some trajectories at high speed with the GP12. Attached is the result from one of the tests I've run. In this test, the time step of the input trajectory was 0.03 seconds (so each time_from_start was 0.03 greater than the prior), the recorded /joint_states values didn't match the input trajectory very well. (In the test when the same trajectory was executed in a lower speed, the two curves overlapped pretty well.) This made me think that the online interpolation part of the robot controller contributed to the slowed down and smoothed out motion. I've also tried interpolating the trajectory to have a timestep of 4ms and sending it without changing any code. It seemed like the computation or data processing somewhere could not be finished in the 4ms, causing the entire trajectory to slow down and giving the Controller is taking too long to execute trajectoryerror. Therefore, I'm thinking maybe skipping the interpolation part could solve the problem.

[EricMarcil]

The might be caused by the internal smoothing of the incoming command. There are ways of reducing this. Please see discussion #458 Implications of high Command - Feedback latency

There are two methods to reduce the latency. These two methods should never be used at the same time. Using them together can result in a motion profile that causes damage to the robot.

The first is to add the HTRAJON command to the robot job. Please see #458 (comment).

The second is to set parameter S2C1584 = 1.

[yueminm]

Hi, sorry for the late update. I've tried the second method, setting parameter S2C1584 = 1, with nothing changed on the MotoPlus application side. Now it does take in waypoints at 4ms intervals and finish the trajectory with no errors. However, the actual trajectory takes much slower than the input trajectory.

[yueminm]

Hi, seems like I would need to use MotoPlus SDK to recompile the code, which I don't have at the moment. I'm wondering how I can get one. Thanks!

[ted-miller]

Apologies for the delayed response.

Please reach out to your local Yaskawa sales representative to receive a quote for the MotoPlus SDK.

Your profile indicates that you are at CMU. I'm not sure, but I believe that someone in your organization might already have a license. So it might be worth asking around internally to see if it has already been purchased.

[yueminm]

Thanks for the info! I'll ask around first and contact the sales representative if we haven't got one already.