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.