Problem operating multiple robot groups at the same time (CSDA10F)

[Danfoa]

Good day,
After following the configuration instructions to use a dual arm Motoman Robot, the CSDA10F, we have encountered a small problem while operating the 15 DOF at the same time, we get an error stating that the start point of the joint_path_command is not the same as the start pose of the robot.

After digging into the problem I know the source of the issue but not how to solve it. The joint_path_command always cero one or the two torso joints (one is a mirror of the other in the URDF). Which triggers the error when the motion_streaming_interface checks the start state.

The support package of the CSDA10F was uploaded by us and can be found here, where the joint_names_csda10f.yaml, and csda10f_motion_interface.yaml are.

Also the moveit controller.yaml configuration we use is here.

Here is the planning command for a 15 DOF motion with the move group csda10f, as you see the last group, which corresponts to one of the torso joints, is cero out.

rostopic echo /joint_path_command
header: 
  seq: 0
  stamp: 
    secs: 1540804125
    nsecs: 866326580
  frame_id: "/base_link"
joint_names: [arm_left_joint_1_s, arm_left_joint_2_l, arm_left_joint_3_e, arm_left_joint_4_u, arm_left_joint_5_r,
  arm_left_joint_6_b, arm_left_joint_7_t, arm_right_joint_1_s, arm_right_joint_2_l,
  arm_right_joint_3_e, arm_right_joint_4_u, arm_right_joint_5_r, arm_right_joint_6_b,
  arm_right_joint_7_t, torso_joint_b1, torso_joint_b2]
points: 
  - 
    num_groups: 4
    groups: 
      - 
        group_number: 0
        num_joints: 7
        valid_fields: 0
        positions: [-0.2465789020061493, 1.3706119060516357, 0.042617324739694595, -0.6750122904777527, 3.111323118209839, 1.1329739093780518, 2.3026256561279297]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        time_from_start: 
          secs: 0
          nsecs:         0
      - 
        group_number: 1
        num_joints: 7
        valid_fields: 0
        positions: [-0.3837849795818329, 0.3498840034008026, -0.48697054386138916, -0.799386203289032, -0.6211710572242737, -0.8425199389457703, -2.3632328510284424]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        time_from_start: 
          secs: 0
          nsecs:         0
      - 
        group_number: 2
        num_joints: 1
        valid_fields: 0
        positions: [-1.2986000776290894]
        velocities: [0.0]
        accelerations: [0.00032456790385969195]
        effort: [0.0]
        time_from_start: 
          secs: 0
          nsecs:         0
      - 
        group_number: 3
        num_joints: 1
        valid_fields: 0
        positions: [0.0]                # This is the error, position, and acceleration should be the same as the joint above.
        velocities: [0.0]
        accelerations: [0.0]
        effort: [0.0]
        time_from_start: 
          secs: 0
          nsecs:         0
  - 

And when I use a motion group that plans only for the two arms (not base), the same error ocures but this time the /joint_path_command topic sends a message just like the one above but with both torso joints cero out. I am not even sure if the torso joints should appear in this case since we are not planning for them.

Can anyone please enlight me on the source of the issue?

PS: Joint states are currently working perfectly, I can acquire the complete robot state, and we are able to move the move groups of 'arm_right', 'arm_left' and 'torso' separately but not combined.

This issue is related to #217,but this time I am personally at the robot and noticed the error source.

[Danfoa]

Sorry to tag you guys but I still dont have any clue how to fix this, @gavanderhoorn @ted-miller, any possible help.

[ted-miller]

Hello Danfoa.
For the SDA type robots, the 3rd and 4th groups must always be identical. The trajectory commands must always be duplicated for these groups when commanding motion. Otherwise, the robot will return an error as you are seeing. I don't have information on how to configure the motion planner to automatically duplicate those groups. I have to defer to @gavanderhoorn or @shaun-edwards .

In regard to moving the two arms without a base, it is the same issue. There are two types of motion commands to the robot. The first a single-group message that will only move one arm and ignore the rest. The other is a multi-group message that will always command all 4 groups. So, even though you don't want to move the torso, your motion command will still contain the position data for the two torso groups. This will result in the error since the 4th group is zeroed out.

[carolMartinez]

Hello,
We are experiencing the same problem that @Danfoa mentions. In our case, we have the SDA10F Dual arm robot. We can plan trajectories for each arm, and torso groups. However, we cannot do it for SDA10F and arms groups.
Does someone know how to solve this problem?
Thanks

[Danfoa]

Good day,
The report from @carolMartinez makes me wonder whether this is an isolated bug we are facing, or rather a feature that has not been yet addressed for dual arm robots from Motoman. Since @shaun-edwards is the most experience person with the SDA10F, I would like to ask him if the control of the 15DOF at the same time has ever been achieved.

I understand the issue comes from ROS not generating trajectory plans for non-actuated joints (which make sense), but then we need to ask:

• If the Yaskawa controller really requires trajectory information for the second base virtual joint, ans something can be done from the Yaskawa's side.
• Is a desirable behavior for the trajectory streamer node (of the motoman driver) to cero out all move groups that are not present on the ROS planned trajectory. This seems to prove wrong when a motion for both arms (and not the torso) is planned, and since none of the base joints are considered into the planned trajectory the streamer node will cero out both of them without considering that the base current position might not be 0. Would it be a better behavior to use the most recent joint state values instead of 0 (maintaining joint position)?
  @ted-miller @gavanderhoorn

[gavanderhoorn]

I only have time for a short reply now, but yes, full body control (to put it like that) with SDA robots has been done before.

A regression in this area may have been introduced in (recent) PRs, that would be something to investigate.

[zzx9636]

Hi,

I am also experience the same issues as @Danfoa and @carolMartinez mentioned above. I have the same error: start point of the joint_path_command is not the same as the start pose of the robot whenever the robot's motion involves more than one move groups.

Does any one know is there a walk-around to achieve 15-DOF motions on SDA10F robots?

Thanks!

[Danfoa]

@zzx9636 I haven't had time to experiment with this issue, I was told by Carol that indigo devel on 2016 was allowing full body operation, but I didn't see much difference between the scripts handling the trajectory generation. I will try to solve this in the next couple of months but If your or Carol have any hints where the problem could be, please share them here.

[gavanderhoorn]

It's very difficult for me to support this, as I don't have access to the hw and cannot simulate it.

As I wrote in #251 (comment), a regression may have been introduced, but at this time I wouldn't know.

[zzx9636]

@Danfoa Thanks for your hints. I can try to get a Ubuntu 14.04 with Indigo and test the full body operation on it. On the mean while, I will spend more time to look into the code and identify the issues.

Thanks!

[gavanderhoorn]

@Danfoa Thanks for your hints. I can try to get a Ubuntu 14.04 with Indigo and test the full body operation on it. On the mean while, I will spend more time to look into the code and identify the issues.

I don't believe @Danfoa wrote that you need to install ROS Indigo. It is more than likely that you can build the indigo-devel branch from source in a Catkin workspace on ROS Kinetic/Xenial.

But seeing as we synced indigo-devel and kinetic-devel a few times, you'll probably need to checkout a historical commit.

Note also this comment by @Danfoa:

I didn't see much difference between the scripts handling the trajectory generation.

[zzx9636]

@gavanderhoorn Thanks for your comment! I will try it out on robot later today!

[ted-miller]

If the Yaskawa controller really requires trajectory information for the second base virtual joint, ans something can be done from the Yaskawa's side.

Technically, yes I could modify MotoROS driver to ignore commands on the second base joint. But, if the motion planner in ROS is not aware of this, then it will lead to a conflict with the feedback positional data being streamed back to the PC.

Is a desirable behavior for the trajectory streamer node (of the motoman driver) to cero out all move groups that are not present on the ROS planned trajectory. This seems to prove wrong when a motion for both arms (and not the torso) is planned, and since none of the base joints are considered into the planned trajectory the streamer node will cero out both of them without considering that the base current position might not be 0. Would it be a better behavior to use the most recent joint state values instead of 0 (maintaining joint position)?

You are correct that a zero value is not appropriate. Instead, the motion planner should monitor the feedback position and command it to remain stationary.

[Danfoa]

You are correct that a zero value is not appropriate. Instead, the motion planner should monitor the feedback position and command it to remain stationary.

@ted-miller And is it necessary for MotoROS to receive command for all DoF of the robot? Why just not send any data at all for Joints that are not desired to be operated, I believe this is how single-group trajectories (i.g. one arm, or the torso) are being send. Am I wrong ?

[ted-miller]

Currently, there are two types of commands. A single-group command and a multi-group command. The single command will always leave the other groups stationary. But, the multi-group command will always control all groups. Currently, the multi-group command has a field to indicate the "number of groups" it is commanding, but it doesn't have a field to allow for non-sequential groups to operate.

For example:
You could command just R1+R2 and allow both torso groups to automatically remain still. But, you couldn't command R1+torso and expect R2 to automatically remain still.

This would require changes on both the MotoROS and ROS-I side to treat the numberOfValidGroups field as a bitfield instead of an integer.

[Danfoa]

Hey to all,

I found a solution to the problem, by changing the default behavior of the Motoman driver when processing a multi-group trajectory with missign robot groups (i.g. 2 arms but no torso motion), as we discussed before:

You are correct that a zero value is not appropriate. Instead, the motion planner should monitor the feedback position and command it to remain stationary. @ted-miller

This is done by changing the lines:

motoman/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp

Lines 276 to 282 in 80c5299

	// Generating message for groups that were not present in the trajectory message
	else
	{
	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);

for:

// Generating message for groups that were not present in the trajectory message
      else
      {
        ROS_DEBUG("Group %s not present in trajectory plan, using its last received joint positions as goal", robot_groups_[group_number].get_name().c_str());
        std::vector<double> positions = last_trajectory_state_map_[group_number]->actual.positions;
        std::vector<double> velocities(num_joints, 0.0);
        std::vector<double> accelerations(num_joints, 0.0);
        std::vector<double> effort(num_joints, 0.0);

By using the last_trajectory_state_map_[group_number]->actual.positions, we are (as far as I understand how Motoman driver works) taking the most recent group joint values received from the YASKAWA robot controller as goal for the current motion trajectory, meaning that the joints that MoveIt did not plan for, will maintain its current position.

@carolMartinez @zzx9636 Please test this and tell me if this solves your problems.

@ted-miller @gavanderhoorn, As soon as I have confirmation from @carolMartinez or @zzx9636 I will create a PR, but I wanted to ask you if you see any way this line of code could affect current behavior for other users...I believe it can be problematic for the ones using on the fly point streaming, but that is only my perception.

[zzx9636]

Hi @Danfoa,

I have just tested on my robot and confirmed that I will able to move the robot with individual arms, torso, both arms and entire 15 DOF without any issues. Earlier today, I have also generated a simple node to modify the moveit trajectory as @ted-miller and @gavanderhoorn suggested and enabled all 15 DOF motions. I will keep you guy noticed if I found any issues in the future!

Thanks you all for your input and help!

[zzx9636]

@Danfoa Sorry for the confusion. I basically write another node to subscribe original joint_path_command and use the current position to replace 0 at motion group 3 and publish it in another topic. It is essentially same idea behind your solution. I have tested your modification and it works great in my application.

[Danfoa]

I was also curious to know if the solution would allow for single arm + torso trajectory execution while keeping the other arm still (obviously relative to the torso joint), as described above:

For example:
You could command just R1+R2 and allow both torso groups to automatically remain still. But, you couldn't command R1+torso and expect R2 to automatically remain still. @ted-miller

so I test it and it does indeed allow it.

Currently I tested motion planning and execution for:

• Single arm + torso
• Single arm
• Dual arm
• All 15 DoF at the same time

All of the above seem to work.
PS: I created a PR (#259) with the code change.

[carolMartinez]

Hello @Danfoa,
I can confirm that with the changes you suggested, we have full body motion of our SDA10F dual-arm robot. Thank you @Danfoa for figuring out a solution. I tested with all the groups, especially with arms (with torso in a position different to 0) and sda10f group. In all the tests, the robot moves without problems.

https://youtu.be/JveSf54NYgI

Thank you guys for all your help.

[gavanderhoorn]

Let's keep this open until #259 is reviewed and merged.

[Danfoa]

As an update I happen to recognize that the action controller is setting the multi-group trajectories commands as ABORTED, right after the new goal has been processed and send to the Yaskawa controller. Even do the real controller is performing the motion as expected, the motion execution is returning:

[ERROR] [/move_group] [1550141776.689035372]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 1.241762 seconds). Stopping trajectory.
[ INFO] [/move_group] [1550141776.689289002]: Cancelling execution for 
[ INFO] [/move_group] [1550141776.689602892]: Completed trajectory execution with status TIMED_OUT ...
[Thread 0x7fff8bfff700 (LWP 12879) exited]
[ WARN] [/move_group] [1550141781.905926934]: Transitioning goal to LOST
[ WARN] [/move_group] [1550141781.906039027]: Controller : no result returned

I will try to address this issue in the following weeks but right now I am a little busy.

Ps: Just to be clear, motion planning and execution is working properly, the issue is (I believe) with the action server for the controller of multi-group motions. I believe the change I introduced is making the action server to not return the goal result, thus the watchdog timer is setting the goal as lost.

[gavanderhoorn]

This seems all related to #226, which discusses these and other issues. See also the issues referenced in #226.

[Danfoa]

@carolMartinez @zzx9636 If you happen to have a little spare time, could you please try PR #259 and verify that multi groups motion execution (start, stop, abortion) are working properly, or if you detect something is missing in the functionality?.