Trajectory start position doesn't match current robot position (3011) when using multiple groups

[rr-dave]

We are trying to control a GP25 and 2 axis positioner with a YRC1000 controller using ROS. We are using the job: https://github.com/ros-industrial/motoman/blob/kinetic-devel/motoman_driver/Inform/DX200%2C%20FS100%2C%20YRC1000/single_arm_with_base_axis/INIT_ROS.JBI on the controller.

We are using 2 control groups like this

---
topic_list:
  - name: robot
    ns: ""
    group: 0
    joints:
      - yaskawa_joint_1_s
      - yaskawa_joint_2_l
      - yaskawa_joint_3_u
      - yaskawa_joint_4_r
      - yaskawa_joint_5_b
      - yaskawa_joint_6_t
  - name: positioner
    ns: ""
    group: 1
    joints:
      - yaskawa_joint_7_p
      - yaskawa_joint_8_y

and using the multi group launch file:

echoing the /joint_state topic shows the robot and positioner joints as expected.

When sending trajectories to the controller we're getting errors like:

[15:48:34.729074] [/yaskawa/motion_streaming_interface]: Aborting Trajectory. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)

I managed to make the GP25 + positioner execute motions by putting all the joints in a single group:

---
topic_list:
  - name: robot
ns: ""
group: 0
joints:
   - yaskawa_joint_1_s
   - yaskawa_joint_2_l
   - yaskawa_joint_3_u
   - yaskawa_joint_4_r
   - yaskawa_joint_5_b
   - yaskawa_joint_6_t
   - yaskawa_joint_7_p
   - yaskawa_joint_8_y

Which suggests the issue isn't with the trajectory. The problem with doing this is that the trajectory timed out even though the robot physically moved. The /joint_state topic also contained 0 for the external axes even when they were jogged on the pendant.

I've tried applying the changes from #64 but this gave the same error. I've also attempted to apply #259 to my fork, but wasn't able to easily get the changes to take. Would the changes in #259 be expected to fix this issue, or are there any other things I can try?

[rr-dave]

I've resolved the above error by making sure the start_state_ in my moveit plan has the same number of joints as the trajectory_. I'm now getting the error:

 [12:02:01.075904] [/yaskawa/move_group]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 1.190000 seconds). Stopping trajectory.

[gavanderhoorn]

This appears to be a MoveIt issue.

The Trajectory Execution Manager (TEM) monitors trajectory execution and apparently your robot is unable to execute the motion in the time MoveIt determined should be needed.

This is often caused by:

• incorrect limits in the URDF/MoveIt configuration, or
• safety features on the Yaskawa controller limiting maximum velocity and/or acceleration which are invisible to the ROS side/application

[rr-dave]

Thanks for the reply. I've tried slowing the motion down considerably and am now getting:

[12:58:56.622997] [/yaskawa/move_group]: Transitioning goal to LOST
[12:58:56.623057] [/yaskawa/move_group]: Controller '' done, no result returned
[12:58:56.623100] [/yaskawa/move_group]: Controller handle  reports status FAILED
[12:58:56.623435] [/yaskawa/motion_server]: ABORTED: CONTROL_FAILED

[gavanderhoorn]

I've tried slowing the motion down considerably

could you comment on what exactly caused the time outs?

am now getting:

[12:58:56.622997] [/yaskawa/move_group]: Transitioning goal to LOST
[12:58:56.623057] [/yaskawa/move_group]: Controller '' done, no result returned
[12:58:56.623100] [/yaskawa/move_group]: Controller handle  reports status FAILED
[12:58:56.623435] [/yaskawa/motion_server]: ABORTED: CONTROL_FAILED

That's the -- currently -- expected error with the action server on multi-group setups and certain configurations.

#488 is the latest attempt at fixing those. #259 was an earlier attempt, but incomplete.

[rr-dave]

could you comment on what exactly caused the time outs?

I can't find the cause here, I've checked the robot controller and there are no alarms or anything.

#488 is the latest attempt at fixing those. #259 was an earlier attempt, but incomplete.

I managed to merge #259 into our fork and am still seeing the same errors. I'll try the same with #488

[gavanderhoorn]

could you comment on what exactly caused the time outs?

I can't find the cause here, I've checked the robot controller and there are no alarms or anything.

I'm confused. You report you've been able to avoid the TEM time-outs by reducing the trajectory velocity.

I assumed that meant you've been able to figure out what was the actual cause. But reading it again it sounds more like a work-around?

#488 is the latest attempt at fixing those. #259 was an earlier attempt, but incomplete.

I managed to merge #259 into our fork and am still seeing the same errors. I'll try the same with #488

I would really recommend against basing any attempts on #259. See my comments there.

[rr-dave]

I'm confused. You report you've been able to avoid the TEM time-outs by reducing the trajectory velocity.

Yes this is just a workaround for now, I'll find a proper fix when I've solved the other issues.

[EricMarcil]

@rr-dave Just to confirm, you are mentionning that you have a positionner with P and Y axes? And you are using a the single_arm_with_base_axis. Base axis means that when you move the P and Y, it moves the robot base in space. You need to match the controller group configuration. Look on the pendant: SYSTEM INFO -> CONTROLLER INFORMATION. What do you have in the CONNECT section?

[rr-dave]

Merging #488 into our fork has fixed the "Transitioning goal to LOST" issues. I've also removed my workaround and haven't seen any TEM timeouts.

@EricMarcil In the CONNECT section we have:

CONNECT:

R1    : 1-06VXH25-A0*
B1    : NONE
R2    : NONE
S1    : TURN-2
S2    : NONE

We chose to use the single_arm_with_base_axis job

• We are using an older version of motoros on the controller
• We were only able to create jobs with certain groups:
  • S1, R1 and NON GROUP
• We made a new group R1 + S1
• Because this is only a single group, it was most similar to the base axis job rather than the external axis

Are there likely to be limitations when using the controller this way, rather than with the external axis job? We're also unaware of the usecase of NON GROUP, is that something you know?

[EricMarcil]

By defaull, the group combinaison R1+S1 might not have been setup. You need to go to menu: SETUP -> GRP COMBINAISON and set it up. I'm assuming that is what you did.
You shouldn't have been able to load the ROS_INIT from the 'single_arm_with_base_axis'. If you manually recreated the job on the pendant for a R1+S1 job, then that should be fine.
For the NON-GROUP, it comes in handy when with have multiple-tasks (which you probably don't need for ROS). If you have multiple tasks, you cannot have more than one task controlling each group. So, if you want a task that just controls some I/O (to other devices like conveyors), you can set that as NON-GROUP.

[rr-dave]

You shouldn't have been able to load the ROS_INIT from the 'single_arm_with_base_axis'. If you manually recreated the job on the pendant for a R1+S1 job, then that should be fine.

Yes this is how we got the job on the controller

We are now able to successfully send trajectories to the robot, however positioner trajectories are often failing with:

[09:07:21.023426] [/yaskawa/joint_trajectory_action]: Outside goal constraints, aborting trajectory

[gavanderhoorn]

Have you verified the position is modelled correctly in your URDF / xacro:macro? Especially the joint limits? Same for whatever motion planner you are using?

[rr-dave]

Thanks for the suggestion, the acceleration limit for the positioner was set too high. I can now reliably send trajectories to the positioner.

I am however having issues with point streaming to the robot. I am sending points to the joint_command topic but no robot movement is happening. The package streaming the points works correctly in a different cell with only the robot connected to the controller. Does anything need to be done differently to stream points in a multi group setup?

[gavanderhoorn]

You're referring to (the original PR) #37 (and/or subsequent rebases) merged with #488?

I'm afraid that's an 'unknown' combination and I would not be able to help you with it.

It's very likely you'll have to extend whatever #37 does/did for multi-group motion. IIRC, the fact it only supported a single group was exactly one of the limitations.

[gavanderhoorn]

I'm going to close this one as I believe the initial issue was resolved (ie: goals getting LOST) by merging in #488.

Follow-up discussion about adapting the point streaming PR(s) for multi-group use happened in #586.

[rr-dave]

Thanks for your help!