Using the Motoman SDA20D Dual-arms DX100 ROS Interface(Indigo)

[gavanderhoorn]

Thanks for the clarification.

If @ted-miller vets your ‘trick’, it’d be nice if we could get that into MotoROS. You’re not the only ones using DX100 controllers with SDA robots, so it’d be nice if we could support that properly.

@andreaskoepf

we have no limitation of moving the torso of our SDA10D with DX100, e.g. we can move all 15 joints, 7 joints of one arm, 14 joints of two arms, or 8 joints of one arm and torso etc

So just for my own understanding: which group on your controller now controls the b1 joint? I was under the impression that the DX100 supported up to 2 groups, but you seem to have 3. Or is the ‘limitation’ in mpMeiIncrementMove(..)?

@andreaskoepf

We are using a slightly modified version of the MotoPlus driver and the Motoman ROS backend (e.g. correcting the deserialization of the joint_feedback_ex message, which also other did with some more lines of code as in Two arms and rail with DX100 PR.

Yes, we need to get those changes (there are more) merged into motoman_driver. They’ve been sitting in a fork for too long.

Would you be willing to test some candidate fixes on your SDA?

[gavanderhoorn]

The jogging interface looks really nice @andreaskoepf.

If the changes are compatible with the other functionality of MotoROS/motoman_driver, it would be great if we could integrate that ‘upstream’. Would you be willing/interested in that?

I haven’t checked your fork yet, but is this similar to ros-industrial/motoman#37?

[andreaskoepf]

@gavanderhoorn Unfortunately our fork is not really in ‘shape’ for easily creating a PR. There are a couple of things that would have to be cleaned and squashed as a lot of debug stuff was checked in which we used to understand what was going on. Our MotoPlus driver changes have not yet been published … e.g. we added a trapezoidal velocity profile controller to the MotoPlus driver which directly moves to streamed set-points without taking a detour over the trajectory spline-interpolation.

(Rosvita has a custom jogging node which allows to jog by twist, catesian-setpoint or joint-directions with safe timeout handling (e.g. to be used via unreliable web-interface) with checking for collision and gracefully handling singularities etc. With the Rosvita UI you can simply enable jogging mode, drag the 3D Cursor at the endeffector and the robot follows.)

[gavanderhoorn]

I’m not necessarily looking for a quick fix/easy grab, but would be interested in getting the changes you describe merged in the main codebase. That way we advance the state-of-the-art and avoid fragmented development, which I believe is very valuable.

If that is something you can agree with, perhaps we can find some way to work towards those goals.

One possibility I see could be for Xamla to apply for a ROSIN FTP. That way you could be (among other things) financially supported to work on the driver.

If that would be of interest, contact me off-list and I can provide you with more information.

[ted-miller]

@andreaskoepf Assuming you still have access to your SDA10D, could you please email me a copy of ALL.PRM and SYSTEM.SYS files. (Please rename the SYSTEM.SYS to SYSTEM.TXT to get past email filters.) There are parameters that control which groups are mapped to the SL-ID slots. I am interested to see how your robot is configured. I don’t have a DX SDA to experiment with right now.

To save the files from the teach pendant:
[Ext Memory] > [Device] > Select CF or USB.
[Ext Memory] > [Save] > “Parameter” > “Batch Parameter”.
[Ext Memory] > [Save] > “System Data” > “System Information”.

[andreaskoepf]

@gavanderhoorn

Would you be willing to test some candidate fixes on your SDA?

We are absolutely interested in collaborating regarding the Motoman ROS driver. Due to the natural situation that most ros-industrial developers only get in touch with a very limited number of systems we see this as crucial.

@ted-miller

could you please email me a copy of ALL.PRM and SYSTEM.SYS files. (Please rename the SYSTEM.SYS to SYSTEM.TXT

Sure, we have sent the files. btw I have pasted the start-up output of the MotoROS driver on our system in a comment to the MotoROS v.1.8.0 PR.

[gavanderhoorn]

The PR for v1.8.0 now contains code that should be close to what you are doing with the two/three motion groups @andreaskoepf.

[Sinai_Aranda]

Hi, I load motoros 1.8v of @ted-miller, I have executed roslaunch successfully. But now have other problem when launch demo (without joint state publisher) with the arm left, already that the joints is missing.

[ERROR] [1527541542.427483218]: State monitor received invalid joint state (number of joint names does not match number of positions)
[ERROR] [1527541542.559572523]: Robot state publisher received an invalid joint state vector
[ERROR] [1527541542.652458005]: Robot state publisher received an invalid joint state vector
[ WARN] [1527541542.682741048]: The complete state of the robot is not yet known.  Missing 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

On rostopic echo /sda20d/sda20d_r1_controller/joint_states yes they are:

header: 
  seq: 54746
  stamp: 
    secs: 1527541767
    nsecs: 419976021
  frame_id: ''
name: ['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']
position: [-0.24274393916130066, 0.8650458455085754, 0.09747595340013504, -0.3556790053844452, -0.25175508856773376, -0.11210209876298904, -0.2976524233818054]
velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: []

But difference on rostopic echo /joint_states the position is empty:

header: 
  seq: 178122
  stamp: 
    secs: 1527541902
    nsecs: 769456976
  frame_id: ''
name: ['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']
position: []
velocity: []
effort: []

[ted-miller]

Hello Sinai,
Can you please try three things:

1. Make sure there is only one instance of MotoROS installed on your controller. Boot into Maintenance mode. Touch [MotoPlus Apl] > [File List]. Make sure there is only one .out file loaded.

2. Please take a picture of the VGA monitor connected to the robot. I would like to see the startup initialization messages.

3. Please run a wireshark capture on the ROS PC. This will record the network communication between the robot and PC. https://jlospinoso.github.io/software/wireshark/networks/ubuntu/2015/02/11/configuring-wireshark-on-ubuntu-14.html

[Sinai_Aranda]

Hi @ted-miller, I changed the init_ros job:

/JOB
//NAME INIT_ROS
//POS
///NPOS 0,0,0,0,0,0
//INST
///DATE 2018/05/30 10:54
///ATTR SC,RW
NOP
PSTART JOB:ROS_R1 SUB1
PSTART JOB:ROS_R2 SUB2
PSTART JOB:ROS_S1 SUB3
PWAIT SUB1
END

This is the wireshark print.

[ted-miller]

The monitor output looks good.

For the wireshark capture, can you please zip the pcapng file and attach it? I am unable to filter and read the raw printed data.

For the INIT_ROS job, I added special jobs for DX100 SDA in my latest commit. https://github.com/ros-industrial/motoman/pull/213/commits/df1256dc6dfd17e518f396cc7aba0517c44c8a62 Please use the jobs in the folder “two_arms_with_base”.

Based on your post, I am assuming that your waist axis is configured as a “station” instead of a “base”. I don’t know if this will work or not. My testing was done using a base-axis. You may need to reconfigure that axis. If you need help with that, please contact Yaskawa Motoman Tech Support and they can provide detailed instructions for your robot.

@gavanderhoorn, do you have any advice about the latest error? Assuming that the robot is indeed reporting the joint states, then I’m not sure what the ROS error messages mean.

[gavanderhoorn]

I first want to see the wireshark capture before looking into this more.

[Sinai_Aranda]

Hi, I configured the waist axis to “base” and I loaded the jobs to robot. The problem persists, this is the wireshark file. Also, I try to monitoring the joint states of real robot in rviz (demo.launch) but now just R2 is correct.

 [ INFO] [1528141714.315548270]: all_joint_state.positions: 0 == all_joint_names: 7
 [ INFO] [1528141714.315751159]: all_joint_state.positions: 1 == all_joint_names: 1
 [ INFO] [1528141714.315910410]: all_joint_state.positions: 7 == all_joint_names: 7

[ERROR] [1528141487.782130639]: State monitor received invalid joint state (number of joint names does not match number of positions)
[ERROR] [1528141487.813911850]: Robot state publisher received an invalid joint state vector

[Sinai_Aranda]

@ted-miller @gavanderhoorn You could connect to my PC trough of teamviewer with the robot on net, to see the problem.

[gavanderhoorn]

I’m currently abroad on travels and cannot assist you directly. I’ll take a look as soon as I can, which may be this weekend or next week.

To me it’s unclear at the moment whether this is a configuration issue on the controller’s side, or whether the ROS side is faulting.

What you could try for now is see whether this pr improves things. If it does, I’ll have a better idea of what is going on exactly. Note that the PR is slightly behind the state of kinetic-devel, but that shouldn’t matter for what you’ll be testing iirc.

[gavanderhoorn]

@ted-miller: just looked at the wireshark capture provided by @Sinai_Aranda. Can you comment on the (unexpectedly) large joint velocities reported for axes J4 to J7 for the third group? Those make little sense to me.

They’re in the third SIMPLEMESSAGE packet of the capture.

[ted-miller]

Based on the wireshark capture, it appears that the robot side is configured correctly. There are three groups and the state appears to be reported correctly (ignoring the velocity data). I do see that [motion_ready] is false… but I assume that’s just because the INIT_ROS job hasn’t been started.

Based on the console output above (all_joint_state.positions: 0 == all_joint_names: 7), I’m speculating one of two things. (But, I fully admit that I don’t know enough about these two things to speak competently; so this is pure speculation.)

1. Configuration issue with the robot definition.
2. For multi-group systems, I send out a Joint Feedback message for each group individually. Additionally, I send out a Multi-group Joint Feedback that contains all groups in one message. Perhaps this version of the ROS-I doesn’t have support for the multi-group message? Maybe it’s incorrectly reading the single-group feedback messages?

In regard to the odd velocity output, I’m not sure. The garbage data only appears on the axes which aren’t used (group 3 is only a single axis). So, I think the problem is that I’m not zero-ing the velocity data prior to calling the mpSvsGetVelTrqFb API. It’s possible that the API is only filling in the first value for the single axis and then ignoring the rest of the values, which is uninitialized data.

[ted-miller]

In regard to the garbage joint velocities, I found this in the reference manual:
“Since the area specified by the parameter is not initialized (0 clear) by this API, initialize the area by the application as necessary.”

So, I just need to clear the data before calling the API. I will fix this in the next commit.

[Sinai_Aranda]

@ted-miller How say PR of @gavanderhoorn:

• If you have a DX100 and a rail, it CANNOT be configured as a base axis if you want to control it with ROS. Instead, you must configure the rail as an external axis.

So, the robot must be configure to station? this way I moved the real robot and rviz same before.

[ted-miller]

If you have a DX100 and a rail, it CANNOT be configured as a base axis if you want to control it with ROS. Instead, you must configure the rail as an external axis

This is apparently not true. andreaskoepf said above that the the SDA should be configured as R1+R2+B1. I have also tested and confirmed that the waist axis should be configured as a base axis.