driver: ~300ms delay between accepting two goals (multi-group setup)

[asierfernandez]

Hi all,

We are experiencing some issues when trying to control simultaneusly two arms of Motoman sda10f robot.

We are able to send a joint_trayectory_action goal to one arm or the other independently, but when we send a goal to both joint trayectory action controllers simuntaneusly, sda10f_r1_controller always blocks. Actually, in our case, the only way to send two trayectories to both arms is applying a short delay of 0.3 seconds (experimentally tested) between two actions goals.

Bellow, a detailed description of our setup:

• Ubuntu 14.04 64 bits with ROS Indigo
• motoman driver: indigo branch, tag 0.3.7
• Motoman SDA10F robot with FS100 controller (version: FS3.27.00A)

In order to properly launch it, first we had to make a change in sda10f configuration file:
Modify motoman_sda10f_support/config/sda10f_motion_interface.yaml
change group index to 0-3, instead of 1-4 to avoid program crash

This is our execution order:

• Connect to the robot_controller through the ethernet communication within the same network.
• Launch MotoRos in robot controller
• Launch sda10f robot related launch files

roslaunch motoman_sda10f_support load_sda10f.launch
roslaunch motoman_sda10f_support robot_interface_streaming_sda10f.launch robot_ip:=10.0.02 controller:=fs100

Any idea of where the problem comes from?

cheers,

asier.

[gavanderhoorn]

Wireshark capture by @asierfernandez (from ROS-Industrial google group posting).

motoman_simultaneus_trayectories.zip

[gavanderhoorn]

Comment by @ted-miller on the ROS-Industrial mailing list:

From the capture, I see that the PC is sending trajectories for a single arm only. There are actually two different trajectory commands; one for a single arm (MsgType 0xE) and another to control multiple arms (MsgType 0x7E0). (Perhaps we should deprecate the single-arm message since the other type has field for "number of groups"...)

So, it appears that there is a configuration issue with your package. I do not personally know the details of how to implement it, but I recommend reviewing the dual-arm tutorial here: http://wiki.ros.org/motoman_driver/Tutorials/Creating%20a%20Dual-Arm%20System

Please note the sentence: *"The config file is loaded by various driver nodes on startup. If this file is missing or not formatted correctly, the driver will default to a single arm mapping." *So I think it is likely there is an issue with your yaml file. An example is provided on the tutorial page.

Also, it may be worth reviewing an active pull request for a user who has been using two separated arm and an additional external axis. #179

If this doesn't help resolve your issue, I recommend that you ping gavanderhoorn on the Github issues page. https://github.com/ros-industrial/motoman/issues

He may be able to provide links to additional resources and information.

I hope this helps.

[gavanderhoorn]

@asierfernandez: it would help if you could make your MoveIt configuration pkg available, and describe any modifications you made to any of the other packages.

As @ted-miller writes, this could be a configuration issue.

[asierfernandez]

I didn't describe well enough our situation.

We are not using MoveIt, we are sending directly action goals to controllers action servers:
/sda10f/sda10f_r1_controller/joint_trajectory_action
/sda10f/sda10f_r2_controller/joint_trajectory_action

If we send a action goal to left server, the left arms moves, and if we send a goal to right server, the right arms moves. The problem comes when we send two goals to two servers at the same time.

Does this description help?

[gavanderhoorn]

@asierfernandez wrote:

Does this description help?

it does, but we're still going to need more information on how you configured things.

As @ted-miller also wrote: the robot state and trajectory relay require certain configuration. It would be good if you could show us how you did that.

[gavanderhoorn]

@asierfernandez wrote:

If we send a action goal to left server, the left arms moves, and if we send a goal to right server, the right arms moves. The problem comes when we send two goals to two servers at the same time.

I'm not sure, but it could be that if you want to execute a trajectory for both arms you should use the combined joint_trajectory_action server, not the two namespaced ones.

The "approximately 0.3 sec" remark makes me suspect the action server, which has a 250 ms periodic thread that implements a state machine responsible for streaming the trajectories to the controller. It could be that after sending your (very) short trajectory to the first arm, it sleeps for 250 ms and then accepts the goal for the other arm.

But I would still think that using the combined server makes more sense for trajectories that involve both arms.

[miguelprada]

Hi @gavanderhoorn, I'm working a bit on this with @asierfernandez.

I don't remember there being a joint_trajectory_action server for both arms, but we will double check tomorrow and get back to you.

In case there isn't by default, is this something that can be configured by modifying sda10f_motion_interface.yaml and adding a group with joints from both arms there, or should this file match some internal configuration in the robot controller and/or MotoROS code? I tried to figure it out from the source, but it's a bit too much for me to digest all at once...

[gavanderhoorn]

I'll take a look tomorrow. Could well be that I'm misremembering and we only had the combined JTA in a private fork.

[asierfernandez]

Hi @gavanderhoorn, following your instructions, I realized that there is an joint trajectory action server with no namespace, just /joint_trajectory_action/ which combines all joints.

Using this server I can send commands to both arms simultaneously.

Thanks for your help!

[gavanderhoorn]

Good to hear that you got it to work.

We should probably track the ~300ms delay between accepting two goals on the namespaced JTAs, as it should not be needed.

[gavanderhoorn]

I've updated the title so we don't need a new ticket.

[gavanderhoorn]

I think it might be nice to namespace the JTA for all joints to avoid this sort of confusion, so I've submitted #189.