Restore and Fix multi-group trajectory execution for multi-group robots

[Danfoa]

The first commit addresses issue #251 which pointed out that multi-group trajectory execution was not working.

With this commit, the default behavior of the Motoman driver when processing a multi-group trajectory with missing robot groups (i.g. When moving two arms but not the torso of the robot, or when moving only the torso and one of the arms) is changed:

• FROM: Zeroing out the trajectory positions for the motion groups for which MoveIt! did not generate trajectory points (move groups that remain stationary on motion planning).
• TO: Use the most recent feedback joint values from these groups, provided by the robot controller.

A simple example to illustrate this issue is to operate both arms of a dual arm robot when the torso joint is not in home position (i.e. joint value != 0). Previously the joint_trajectory_action was creating the dual arm trajectory with the torso joint trajectory value set always to 0, since MoveIt! does not send any data this joint. When MotoROS receives this trajectory and tries to execute it it realizes that the initial state of the motion plan differs from the current robot state, since the torso joint is not at 0, and rejects motion execution command.

[Danfoa]

The Second Commit addresses the issue explained in #251 in which multi-group trajectories are never set as succeded by the JointTrajectoryAction server. It also addresses the lack of error checking and incompleteness in the processing of multi-group goals explained in issue #226.

In summary I:

• Implement watchdog timer to check the entire robot (all robot groups) feedback state reception.
• Increase error checking for "global"/"multi-group" trajectory goals for the JointTrajectoryAction server (issue driver: difference between global AS and per-group AS #226).
• Implement goal abortion/cancel capabilities for the "global/multi-group" JointTrajectoryAction server. (issue driver: difference between global AS and per-group AS #226)
• Implement JointTrajectoryAction::withinGoalConstraints function for "global"/"multi-group" trajectory goals. Note: previous implementation of this function was not working. (related to issue Problem operating multiple robot groups at the same time (CSDA10F) #251)

I introduced a new variable in the joint_trajectory_action.h header file, in order to keep a record of all groups active in the current multi-group goal:

   /**
   * \brief Indicates which groups are active on `multi-group` action goals, 
   * Note: independant of `has_active_goal_map_` which handles single group goals.
   */
  std::map<int, bool> multi_group_active_goals_map_;  

Results:

I have tested several multi-groups and single-group trajectories on the Motoman CSDA10F; it seems that everything is working properly now for both types of motion (single-group trajectories are not affected by the changes in this pull request).

Multi-group motions now can be:

• Cancelled/Aborted properly (internall feedback checking stops). Note: Robot motion is not stopped on abortion, but that is the normal behaviour of the motoman driver) with single trajectories too
• Labeled as succedded by the action server when all robot groups reach the desire position. (This was not working before)
• Aborted if there are robot groups which feedback values havent been received.

[gavanderhoorn]

@Danfoa: many thanks for the PR. A quick look seems to suggest it does address many of the problems I documented in the issues you link in the description.

I'll try to get to this next week.

[tdl-ua]

I have tested this PR on a MA2010 and Motopos D500 and am able to move both the manipulator and positioner.

The motions seem to be completed but at the end of each motion, the following error is thrown:
Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 3.539739 seconds). Stopping trajectory.

[Danfoa]

@tdl-ua could I ask you to check the console stream in debug level, and look if anything looks odd?

Most likely the withinGoalConstrains implementation for multi-group trajectories is not working for you specific robot configuration, but I do not know why. Having the output stream and a notion of how your robot is configured might shed some light on the problem.

[gavanderhoorn]

@tdl-ua could I ask you to check the console stream in debug level, and look if anything looks odd?

Most likely the withinGoalConstrains implementation for multi-group trajectories is not working for you specific robot configuration, but I do not know why. Having the output stream and a notion of how your robot is configured might shed some light on the problem.

@tdl-ua has a 2 group system, which is different from the dual-arm setup that you've been working on @Danfoa.

Goal completion checking is similar, but I believe your current code has some implicit assumptions that do not hold for < 4 group systems.

I'm currently restoring the multi-group config of my GP25 and should be able to finally get to a review of your PR.

[Danfoa]

I believe your current code has some implicit assumptions that do not hold for < 4 group systems.

I tried to avoid these implications. Re-checking my code I am not able to spot the possible source of error.
I'll be expecting your feedback.

[Danfoa]

Friendly reminder

[tdl-ua]

@Danfoa apologies for the radio silence. I'll do those checks asap.

[Danfoa]

@tdl-ua As a matter of fact, I am able to perform two-group (one arm and torso, or both arms) and tree-group (two arms and torso) motions, without any issues. I did notice that when I reduce the overall speed of the motion planning (moveit max speed settings) the error Controller is taking too long to execute trajectory sometimes occurs, could you please tell me if this error occurs always? without regards to the speed of the motion?

[tdl-ua]

@Danfoa I've just tested some motions again and it seems to occur with every single one.

I will try to make some time later this week to finally do some digging on this issue. I'll keep you posted.

[tdl-ua]

@Danfoa I've just recently updated MotoROS on our DX200 controller to version 1.8.2 and that seemed to fix the "Controller is taking too long to execute trajectory" error. I've tested multiple motions and the error was not thrown after completing any of them.

[gavanderhoorn]

@Danfoa I've just recently updated MotoROS on our DX200 controller to version 1.8.2 and that seemed to fix the "Controller is taking too long to execute trajectory" error. I've tested multiple motions and the error was not thrown after completing any of them.

1.8.2 is definitely needed, as it contains fe26038, which fixes the in_motion flag coming from MotoROS. This didn't work correctly for multi-group systems (or at least, it didn't work as the ROS side expected/required).

[gavanderhoorn]

@Danfoa: finally had a chance to test this.

On a setup with 2 groups (1 robot, 1 linear axis) planning and execution works for the first group, but not when a goal is given the for the entire robot (ie: all joints of the robot + the linear axis).

Planning works, but attempting to execute trajectories results in:

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

which I believe is #111.

I'll see if I can check tomorrow whether it is.

[gavanderhoorn]

Just noting this here: a fix for what 72cd3af attempts to fix (groups not present being zeroed out completely) was already submitted by @thiagodefreitas in #64 (which ended up in hydro-devel in 2015-05-07).

I'm not entirely sure why that code did not make it into kinetic-devel.

A git diff hydro-devel kinetic-devel -- motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp clearly shows the (unintended?) revert:

@@ -333,10 +276,10 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
       // Generating message for groups that were not present in the trajectory message
       else
       {
-        std::vector<double> positions=last_trajectory_state_map_.at(rbt_idx)->actual.positions;
-        std::vector<double> velocities = last_trajectory_state_map_.at(rbt_idx)->actual.velocities;
-        std::vector<double> accelerations = last_trajectory_state_map_.at(rbt_idx)->actual.accelerations;
-        std::vector<double> effort = last_trajectory_state_map_.at(rbt_idx)->actual.effort;
+        std::vector<double> positions(num_joints, 0.0);
+        std::vector<double> velocities(num_joints, 0.0);
+        std::vector<double> accelerations(num_joints, 0.0);
+        std::vector<double> effort(num_joints, 0.0);
 
         dyn_group.positions = positions;
         dyn_group.velocities = velocities;

#64 actually copies the entire state for groups not part of the goal, not just positions (velocities, accelerations and effort).

Not sure that is needed (as not-included groups are most likely assumed to be not moving), but it's good from a consistency point-of-view.

---

Edit: looks like indigo-devel was last synced with hydro-devel in #61, which doesn't include the commits from #64:

[gavanderhoorn]

@Danfoa: I'm just making notes of this for myself here btw. This is not something you necessarily need to act on.

[Danfoa]

Just noting this here: a fix for what 72cd3af attempts to fix (groups not present being zeroed out completely) was already submitted by @thiagodefreitas in #64 (which ended up in hydro-devel in 2015-05-07).

I'm not entirely sure why that code did not make it into kinetic-devel.

A git diff hydro-devel kinetic-devel -- motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp clearly shows the (unintended?) revert:

@@ -333,10 +276,10 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
       // Generating message for groups that were not present in the trajectory message
       else
       {
-        std::vector<double> positions=last_trajectory_state_map_.at(rbt_idx)->actual.positions;
-        std::vector<double> velocities = last_trajectory_state_map_.at(rbt_idx)->actual.velocities;
-        std::vector<double> accelerations = last_trajectory_state_map_.at(rbt_idx)->actual.accelerations;
-        std::vector<double> effort = last_trajectory_state_map_.at(rbt_idx)->actual.effort;
+        std::vector<double> positions(num_joints, 0.0);
+        std::vector<double> velocities(num_joints, 0.0);
+        std::vector<double> accelerations(num_joints, 0.0);
+        std::vector<double> effort(num_joints, 0.0);
 
         dyn_group.positions = positions;
         dyn_group.velocities = velocities;

#64 actually copies the entire state for groups not part of the goal, not just positions (velocities, accelerations and effort).

Not sure that is needed (as not-included groups are most likely assumed to be not moving), but it's good from a consistency point-of-view.

I did assume that non-present groups in the plan should remain still, but I understand that for consistency and maybe future flexibility the default state should replicate the position, velocities, accelerations, and efforts of the most recent robot state received. I could include this line on the PR if you believe is appropriate.

Planning works, but attempting to execute trajectories results in:

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

which I believe is #111.

I am almost sure this error is unrelated to my code because even if a group is left out of the multi-group trajectory, its joint value would (with this PR) default to the current robot position. In my experience, this happened because of #111 or because the motion plan was being done with an old value of the robot state (the robot was still moving when the planning was started).

I am sorry to inform you that I no longer have access to a YASKAWA robot, so I cannot test or debug anymore the functionality of this code.

[gavanderhoorn]

I am almost sure this error is unrelated to my code because even if a group is left out of the multi-group trajectory, its joint value would (with this PR) default to the current robot position. In my experience, this happened because of #111 or because the motion plan was being done with an old value of the robot state (the robot was still moving when the planning was started).

Yes, I'm also pretty sure it's not related to the changes in this PR, but I wanted to make sure.

I am sorry to inform you that I no longer have access to a YASKAWA robot, so I cannot test or debug anymore the functionality of this code.

That won't be a problem. I do, so I can test this.

I'm already happy you contributed the PR.

[akashjinandra]

Hello, are there any plans to merge this back to main branch?

[jmarsik]

Hello @gavanderhoorn, is there any reason why this PR is still not merged? Could we perhaps help with it? What is missing? Of course the first step would be to get it updated or rebased.

We have a GP180-120 robot on a custom linear track/rail that is being configured and I believe that we will hit mentioned problems too. Our track is configured as a RECT_X rack&pinion type axis in the YRC1000 controller and that means it's multi-group type of configuration on the MotoROS level, correct? Is there any other possibility?

I've been researching related issues and PRs back and forth and I believe that some of them could be probably solved simply by using multiple MoveIt "controllers" in MoveIt configuration, but maybe not not all of them.

[gavanderhoorn]

The reason this isn't merged is because it's incomplete. It doesn't deal with a couple of cases where the code makes assumptions about joint order and group ID matching to index in the vectors used.

IIRC, this essentially makes this PR only work reliably for SDA configurations, not for anything with > 1 but < 4 motion groups.

The reason it hasn't been closed is because there is still value in keeping the proposed changes around. Closing would make it 'invisible'. In that sense this is currently a TODO.

[jmarsik]

Ok, I understand, I have also looked at the code and you are right. Maybe we can help with that in the coming days.

What would be the best approach? Get the code from @Danfoa branch and create a new PR? Or maybe @Danfoa could give me (and maybe one or two of my colleagues) contributor permissions to his branch so that we can keep it under this PR?

[gavanderhoorn]

Even though this PR is incomplete, it does work for many setups.

I've been working through outstanding PRs the past couple days, and plan to get to this one as well.

While I appreciate the offer (thanks for that!), perhaps the most efficient way to contribute would be to test a more complete version of the changes this PR proposes, once I have them ready.

[gavanderhoorn]

And because I feel it's important: we're still grateful @Danfoa took the time to figure out what was missing and submit this PR.

The code this touches is very old, and while not overly complex, it also isn't the most straightforward implementation sometimes.

[jmarsik]

Ok, no problem. Feel free to let me know here if we can help in any way.

We will definitely try the changes now and then again with a more complete version, because yesterday we tried kinetic-devel code with our setup and got LOST joint trajectory action goal error every time. Even when we commanded all 7 axes together. Originally I thought that this error only appears when I send a goal with some joints missing, but I was wrong.

[gavanderhoorn]

Conceptually, the changes needed would be (similar to) the ones included in #179.

I've personally used those as part of the APC setup mentioned in the comments.

[gavanderhoorn]

@jmarsik: kinetic-devel...gavanderhoorn:rebased_two_arms_on_a_rail is I believe the minimum set of changes needed to resolve the LOST goal problem for setups like yours.

Those changes are from #179, but actually come from a much older branch (as mentioned in the comments).

I've rebased #179 on-top of current kinetic-devel.

It's all not very nice, but we can work on improving it later.

[jmarsik]

@jmarsik: kinetic-devel...gavanderhoorn:rebased_two_arms_on_a_rail is I believe the minimum set of changes needed to resolve the LOST goal problem for setups like yours.

Those changes are from #179, but actually come from a much older branch (as mentioned in the comments).

I've rebased #179 on-top of current kinetic-devel.

It's all not very nice, but we can work on improving it later.

Hello, I have made some progress with the code, see branch https://github.com/jmarsik/motoman/tree/kmr-multigroup ... I have merged your coalesced_joint_feedback_ex_relay into rebased_two_arms_on_a_rail and then implemented changes to make it work on our setup. Actually I think that those changes are in some way similar to @Danfoa changes in this PR, but are based on your branch.

Then I continued with support for individual goal tolerances (in real setups you definitely want them different for linear axes in mm and rotational axes in rad).

And then I enhanced JTA logging a little bit because it was a mess. There was no way to recognize if the message came from individual group handling or "global" multigroup handling.

I still see a lot of room for improvement.

[jmarsik]

@jmarsik: kinetic-devel...gavanderhoorn:rebased_two_arms_on_a_rail is I believe the minimum set of changes needed to resolve the LOST goal problem for setups like yours.
Those changes are from #179, but actually come from a much older branch (as mentioned in the comments).
I've rebased #179 on-top of current kinetic-devel.
It's all not very nice, but we can work on improving it later.

Hello, I have made some progress with the code, see branch https://github.com/jmarsik/motoman/tree/kmr-multigroup ... I have merged your coalesced_joint_feedback_ex_relay into rebased_two_arms_on_a_rail and then implemented changes to make it work on our setup. Actually I think that those changes are in some way similar to @Danfoa changes in this PR, but are based on your branch.

Then I continued with support for individual goal tolerances (in real setups you definitely want them different for linear axes in mm and rotational axes in rad).

And then I enhanced JTA logging a little bit because it was a mess. There was no way to recognize if the message came from individual group handling or "global" multigroup handling.

I still see a lot of room for improvement.

Did you have a chance to look at it? We are using it for some time with our setup, at the moment only with the "global" action server. And it works perfectly. We will test the "individual" action servers and the proper interaction between them and the "global" one probably in the coming weeks.