Aborting Trajectory. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)

[akashjinandra]

Hello, My system: ROS Melodic, Ubuntu 18.04, YRC1000 running MotoRos 1.99 I'm running a multi group system with a robot on a track, meaning a group for robot and a group for the track. I am running into this error: Aborting Trajectory. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)

I am currently on this branch: https://github.com/ros-industrial/motoman/pull/259

The error doesn't seem to happen very frequently. I've read into: https://github.com/ros-industrial/motoman/issues/251 and https://github.com/ros-industrial/motoman/issues/111 however it doesn't seem like there is a solution to this issue. But I might be getting lost in reading the issues and possible solutions people have tried. Is there any solution/test code I can use to help debug this?

[gavanderhoorn]

@akashjinandra: I've been able to reproduce the crash. I don't believe I would need you to test anything right now.

Having some representative JointTrajectorys would be helpful though.

---

Edit: could you confirm you see the crash when you're trying to execute a trajectory for a (MoveIt) group which doesn't include all joints (ie: only the robot's joints, or the robot's + one of the gripper joints)?

---

Edit 2: after adding a check (to make sure trajectory.joint_names == all_joint_names_) the JTA no longer crashes when sent partial goals.

I assume however that you'd like to be able to submit goals for specific groups individually @akashjinandra, correct? Technically, the global JTA was not supposed to be used for that I believe (but I was not involved in the design, nor the implementation). But I also don't believe you can send goals to the per-group action servers at the same time (which could perhaps offer a work-around, as MoveIt can deal with multiple controllers).

The changes in https://github.com/ros-industrial/motoman/compare/kinetic-devel...gavanderhoorn:rebased_two_arms_on_a_rail do not add partial-goal capability. #259 does. That's likely why you see the crashes.

[gavanderhoorn]

@akashjinandra: please ignore my previous request, and please test https://github.com/ros-industrial/motoman/compare/kinetic-devel...gavanderhoorn:coalesced_joint_feedback_ex_relay merged into #259. https://github.com/ros-industrial/motoman/compare/kinetic-devel...gavanderhoorn:pr259_rebased_with_coalesced_feedback.

My assumption is the changes in that branch will help with the outdated JointState msgs MoveIt is trying to use.

---

Edit: I've gone ahead and rebased #259 on-top of current kinetic-devel and merged gavanderhoorn:coalesced_joint_feedback_ex_relay into it.

I cannot stress-test it in the same way you are @akashjinandra, so let me know whether this solves your current issue.

[gavanderhoorn]

Friendly ping @akashjinandra.

[akashjinandra]

Hello @gavanderhoorn we tested this driver: https://github.com/ros-industrial/motoman/pull/488 and it seems to be working but are doing a couple more tests today to say for sure. I will also try out the one that you have created for us.

[gavanderhoorn]

The changes proposed in #488 are similar to the ones in #259, with some additions. It's also incomplete, just as #259 is/was.

If it works for you, it works of course.

The branch I created only tries to address the problem of out-of-date JointState messages. It's not a fix for all the known problems.

I would still like to know whether the changes to the multi-group publishers help avoid the out-of-date JointState messages, so if you could test that, that would be great.

[akashjinandra]

Hey @gavanderhoorn I'm trying to look at the joint state topics it seems like there aren't any separate ones for each joint group they are all in one message now, how would you like me to verify that this problem is fixed?

[gavanderhoorn]

There should still be separate topics. That hasn't changed.

What did change is that the global topic (ie: /joint_states) now carries JointState messages which are the coalesced version of the messages on the individual topics.

Could you show me a rostopic list | grep joint_states and a rostopic echo -n1 /joint_states (provided /joint_states is the topic published to by motoman_driver).

how would you like me to verify that this problem is fixed?

whatever you were doing earlier, and which caused the problem to occur.

I suspect the problem with the outdated JointState messages was caused by essentially starvation. The publishers all had queue sizes of 1, including the global topic, and it's possible MoveIt / your program(s) just wasn't able to receive all the messages it needed. This would cause outdated state to be used when setting up new FollowJointTrajectoryAction goals, leading to motoman_driver reporting the error that started this thread.

In all cases you reported, MotoROS was correct in that the start state of the trajectory did not match the current state. Incomplete multi-group support was not the cause there. Outdated JointStates being used is a much more likely cause.

[gavanderhoorn]

Friendly ping @akashjinandra.

If you can't find an opportunity to test this any more, also ok, but please let me know.

[gavanderhoorn]

I'm going to assume you got the immediate issue fixed and don't/can't spend time testing https://github.com/ros-industrial/motoman/compare/kinetic-devel...gavanderhoorn:pr259_rebased_with_coalesced_feedback any more.

Closing.