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

I’m not entirely sure, but I believe you’re not running the multi-group version of the driver.

Message 2017 is ROS_MSG_MOTO_JOINT_FEEDBACK_EX (from REP-I0004), which is the message used by the driver to report multi-group joint states. Unless the multi-group codepaths are used in motoman_driver, the ROS nodes will not know how to deserialise those messages and the multi-group interface will not work.

Can you please provide some more details on what exactly you are launching, where you got it from, and what parameters you are providing it (ie: packages, launch files, yaml configuration files, etc).

IIRC, the group for the torso should not have all those joints.

1 Like

Hi, it is MotoROS v1.7.0 and Motoman Driver package is this. I created urdf and moveit package (motoman_sda20d_config). I follow this tutorial but I do not find away to rename joints of groups about robots. I launch:

roslanch motoman_driver robot_multigroup_interface_streaming_dx100.launch robot_ip:= 192.168.255.10

Also,

roslaunch motoman_driver robot_interface_streaming_dx100.launch robot_ip:=192.168.255.10

In rostopic /joint_states it shows:

[‘joint_1’, ‘joint_2’, ‘joint_3’, ‘joint_4’, ‘joint_5’, ‘joint_6'] for arm left, right and torso.  

how I can do configuration for robot with 2 groups of arms and torso?

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
1 Like

after,

rosparam load /motoman_sda20d/urdf/motoman_sda20d.urdf robot_description

and

rosparam set controller_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]"

How can I do the correct configuration?

1 Like

Did you have a reason for deleting your two posts?

@Sinai_Aranda: perhaps you can use Danfoa/invite-robotics as inspiration. It is for an SDA10, but that should be almost a search->replace action at this level.

The invite_motoman_support pkg shows you how to configure the controller_joint_names and the other required parameters.

Note that the pkgs in Danfoa/invite-robotics contain a lot of other things (for integration of a gripper fi) that you can ignore. Also note that they are essentially a customised version of the pkgs in ros-industrial/motoman and ros-industrial/motoman_experimental.

1 Like

hi, I have the new configuration for package, now I can see the joint states correct of right arm, left arm and torso. But only run one time, and it’s show this:

started roslaunch server http://sda20-Aspire-S3:33061/

SUMMARY
PARAMETERS
 * /robot_ip_address: 192.168.255.10
 * /rosdistro: indigo
 * /rosversion: 1.11.21
 * /topic_list: [{'joints': ['arm...

NODES
  /
    joint_state (motoman_driver/robot_state)
    joint_trajectory_action (motoman_driver/motoman_driver_joint_trajectory_action)
    motion_streaming_interface (motoman_driver/motion_streaming_interface)

auto-starting new master
process[master]: started with pid [7205]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to c9baf556-51fd-11e8-8a16-009a9e9d9347
process[rosout-1]: started with pid [7218]
started core service [/rosout]
process[joint_state-2]: started with pid [7228]
process[motion_streaming_interface-3]: started with pid [7236]
process[joint_trajectory_action-4]: started with pid [7243]
_**[FATAL] [1525701109.760904112]: ASSERTION FAILED**_
_**	file = /home/sda20/cidesi_robots_ws/src/motoman-indigo-devel/motoman_driver/src/industrial_robot_client/joint_relay_handler.cpp**_
_**	line = 320**_
_**	cond = all_joint_state.positions.size() == all_joint_names.size()**_

_**[joint_state-2] process has died [pid 7228, exit code -5, cmd /home/sda20/cidesi_robots_ws/devel/lib/motoman_driver/robot_state __name:=joint_state __log:=/home/sda20/.ros/log/c9baf556-51fd-11e8-8a16-009a9e9d9347/joint_state-2.log].**_
_**log file: /home/sda20/.ros/log/c9baf556-51fd-11e8-8a16-009a9e9d9347/joint_state-2*.log**_

Will it be from the motoplus or this configuration?

Can you show us the value of the controller_joint_names parameter?

And could you please post the output of roslaunch when you start it with the --screen command line option?

$ roslaunch motoman_sda20d_moveit_config robot_interface_streaming_sda20d.launch --screen
... logging to /home/sda20/.ros/log/55952d18-52d4-11e8-93b2-009a9e9d9347/roslaunch-sda20-Aspire-S3-15122.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://sda20-Aspire-S3:43152/

SUMMARY

PARAMETERS
 * /controller_joint_names: ['arm_left_joint_...
 * /robot_ip_address: 192.168.255.10
 * /rosdistro: indigo
 * /rosversion: 1.11.21
 * /topic_list: [{'joints': ['arm...

NODES
  /
    joint_state (motoman_driver/robot_state)
    joint_trajectory_action (motoman_driver/motoman_driver_joint_trajectory_action)
    motion_streaming_interface (motoman_driver/motion_streaming_interface)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[joint_state-1]: started with pid [15140]
process[motion_streaming_interface-2]: started with pid [15141]
[ INFO] [1525793277.774871649]: Added message handler for message type: 0
process[joint_trajectory_action-3]: started with pid [15150]
[ INFO] [1525793277.783318367]: Robot state connecting to IP address: '192.168.255.10:50241'
[ INFO] [1525793277.786943671]: Loading topic list
[ INFO] [1525793277.787028643]: Found 4 topics
[ INFO] [1525793277.787093476]: Topic(state_value): [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525793277.787155804]: Topic(state_value): [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525793277.787214808]: Topic(state_value): [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525793277.787274280]: Topic(state_value): [group:3,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525793277.787322502]: Loading group: [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525793277.787537892]: Loading group: [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525793277.787624508]: Loading group: [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525793277.787683830]: Loading group: [group:3,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525793277.787750856]: Loaded 4 groups
[ INFO] [1525793277.787833872]:  Initializing robot state 4 groups
[ INFO] [1525793277.787894632]: Initializing message manager with default comms fault handler
[ INFO] [1525793277.787941047]: Default communications fault handler successfully initialized
[ INFO] [1525793277.787979140]: Initializing message manager
[ INFO] [1525793277.788027577]: Added message handler for message type: 1
[ INFO] [1525793277.809585729]: Added message handler for message type: 10
[ INFO] [1525793277.810346040]: Joint Trajectory Interface connecting to IP address: '192.168.255.10:50240'
[ INFO] [1525793277.813892230]: Added message handler for message type: 15
[ INFO] [1525793277.813995011]: Creating joint_feedback_ex_relay_handler with 4 groups
[ INFO] [1525793277.819683274]: Loading topic list
[ INFO] [1525793277.819847125]: Found 4 topics
[ INFO] [1525793277.819924352]: Topic(state_value): [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525793277.819994004]: Topic(state_value): [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525793277.820046525]: Topic(state_value): [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525793277.820089922]: Topic(state_value): [group:3,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525793277.820134699]: Loading group: [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525793277.820247098]: Loading group: [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525793277.820320855]: Loading group: [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525793277.820388747]: Loading group: [group:3,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525793277.820461235]: Loaded 4 groups
[ INFO] [1525793277.820530037]: MotomanJointTrajectoryStreamer: init
[ INFO] [1525793277.820582817]: JointTrajectoryStreamer: init
[ INFO] [1525793277.821819561]: Connected to server
[ INFO] [1525793277.829682953]: Loading topic list
[ INFO] [1525793277.829783638]: Found 4 topics
[ INFO] [1525793277.829828822]: Topic(state_value): [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525793277.829886254]: Topic(state_value): [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525793277.829948569]: Topic(state_value): [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525793277.830012649]: Topic(state_value): [group:3,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525793277.830065815]: Loading group: [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525793277.830190502]: Loading group: [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525793277.830258075]: Loading group: [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525793277.830316122]: Loading group: [group:3,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525793277.830378247]: Loaded 4 groups
[ INFO] [1525793277.830492358]: Added message handler for message type: 2017
[ INFO] [1525793277.833911412]: Connected to server
[ INFO] [1525793277.833981932]: Successfully initialized robot state interface
[ INFO] [1525793277.834048878]: Entering message manager spin loop
[ INFO] [1525793277.939026704]: Unlocking mutex
[ INFO] [1525793277.939193526]: Starting Motoman joint trajectory streamer thread
[ INFO] [1525793277.944351291]: Connecting to robot motion server
[ WARN] [1525793277.944421356]: Tried to connect when socket already in connected state
[FATAL] [1525793278.015168488]: ASSERTION FAILED
	file = /home/sda20/cidesi_robots_ws/src/motoman-indigo-devel/motoman_driver/src/industrial_robot_client/joint_relay_handler.cpp
	line = 320
	cond = all_joint_state.positions.size() == all_joint_names.size()

[joint_state-1] process has died [pid 15140, exit code -5, cmd /home/sda20/cidesi_robots_ws/devel/lib/motoman_driver/robot_state __name:=joint_state __log:=/home/sda20/.ros/log/55952d18-52d4-11e8-93b2-009a9e9d9347/joint_state-1.log].
log file: /home/sda20/.ros/log/55952d18-52d4-11e8-93b2-009a9e9d9347/joint_state-1*.log

In new terminals (these just it’s show before of the error):

$ rostopic echo /sda20d/sda20d_r1_controller/joint_states 
header: 
  seq: 0
  stamp: 
    secs: 1525791870
    nsecs: 125187133
  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.32970359921455383, 0.983213484287262, -0.030645526945590973, -1.0131772756576538, -1.3398334980010986, -1.8006503582000732, 1.0624170303344727]
velocity: []
effort: []

$ rostopic echo /sda20d/sda20d_r2_controller/joint_states 
header: 
  seq: 0
  stamp: 
    secs: 1525791870
    nsecs: 123301906
  frame_id: ''
name: ['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']
position: [-0.08535750955343246, 1.1426793336868286, 0.18094155192375183, -1.0275626182556152, -0.9328728914260864, -1.4836479425430298, 0.06319399178028107]
velocity: []
effort: []

$ rostopic echo /sda20d/sda20d_b1_controller/joint_states 
header: 
  seq: 0
  stamp: 
    secs: 1525791870
    nsecs: 125437761
  frame_id: ''
name: ['torso_joint_b1']
position: [0.06071678176522255]
velocity: []
effort: []

$ rostopic echo /sda20d/sda20d_b2_controller/joint_states

In display of the robot Motoplus:

Controller number of group = 3
StateServer Send failure. Closing state server connection.
Closing Motion Server Connection.
Motion Server Connection Closed.

Exist a problem with motoman_driver related with driver: assertion in joint_relay_handler when controller has < 4 motion groups configured and Removed version0 parameters (as much as possible)?
I used the last motoman_driver version for indigo-devel.

I’m wondering whether there is some misconfiguration / misunderstanding here.

@ted-miller: afaik, all SDAx robots have 4 groups, with groups 2 & 3 controlling the two torso joints, correct? How is this configured on the DX100?

In the screenshot provided by @Sinai_Aranda, it looks like only 3 AddToIncQueue tasks are created. This strikes me as odd.


Edit: @Sinai_Aranda: could you test with group nr 3 (so the fourth entry) removed from your sda20d_motion_interface.yaml

A wireshark capture of some traffic would also help.

@gavanderhoorn I can confirm that a SDA can be operated with three groups on a DX100. Removing the shared torso axis was the only way we found to execute full body motions (both arms + torso) with a DX100. Primary reason for this is the Motoplus driver which has to split the updates for both robots (R1, R2) on the DX100 into two different calls, e.g. see MotionServer.c#L1693-L1715. If both calls are executed after each other the DX100 complains about different joint values for the shared base axis (even when you try to set the exact same values).

Just one important thing regarding motoman/#91 (>1 but <4 groups): see my quickfix here.

@gavanderhoorn The SDA operates differently on a DX100 than an FS100 or above.

The DX100 version of MotoROS can only support 2 control groups. This is a limitation of the API which controls motion (mpMeiIncrementMove). So, for an SDA robot, you cannot control both arms and also the torso.

I just realized that the max-group-detection mechanism in MotoROS doesn’t have the restriction for DX100. I have just updated PR #213 to include this detection. In this new version, it will not create a task for the third group.

I’m confused by @andreaskoepf’s comment. Are you saying you were able to control all three groups with MotoROS on a DX100?

@ted-miller: ok, this makes sense for me again now.

So how are the motion groups configured then for the SDA on DX100?

We should document this on the wiki.

@andreaskoepf: do I understand you correctly that you configured things such that you can only control both arms, but not the torso also on DX100?

@ted-miller I did this some time ago. I also had to tweak the Motoplus driver a bit to make this work, especially in MotionServer.c I added this:

// first robot
moveData.ctrl_grp = 1 | (1 << 2);		// AKo: Hard coded special value for our SD10D R1+B1 R2 special configuration
ret = mpMeiIncrementMove(MP_SL_ID1, &moveData);

But yes, with these changes it is definitely possible to operate all 15 joints of the SDA10D with a DX100 controller simultaneously (run whole body motions).

We at Xamla added a couple of more things to the Motoplus driver for better realtime control etc, e.g. see FT control or joystick control. In general an issue of the DX100 is the big latency of the system (~ 100ms) which is probably caused by lower level moving average filtering in the motor controllers.

@andreaskoepf This can apply on indigo-devel (here)? The difference it’s that the first take i=MOT_MAX_GR -1 it’s decreasing. The other from zero to until number groups.

@gavanderhoorn I have eliminated group three and two for arms, but I have the same problem:

[ INFO] [1525802331.438267018]: Added message handler for message type: 0
[ INFO] [1525802331.443545844]: Robot state connecting to IP address: '192.168.255.10:50241'
[ INFO] [1525802331.447706704]: Loading topic list
[ INFO] [1525802331.447797778]: Found 4 topics
[ INFO] [1525802331.447990373]: Topic(state_value): [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525802331.448174949]: Topic(state_value): [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525802331.448349955]: Topic(state_value): [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525802331.448586850]: Topic(state_value): [group:2,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525802331.448777123]: Loading group: [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525802331.449097947]: Loading group: [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525802331.449303266]: Loading group: [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525802331.449477517]: Loading group: [group:2,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525802331.449663703]: Loaded 3 groups
[ INFO] [1525802331.449878726]:  Initializing robot state 3 groups
[ INFO] [1525802331.450044933]: Initializing message manager with default comms fault handler
[ INFO] [1525802331.450200052]: Default communications fault handler successfully initialized
[ INFO] [1525802331.450351758]: Initializing message manager
[ INFO] [1525802331.450507301]: Added message handler for message type: 1
[ INFO] [1525802331.452929789]: Joint Trajectory Interface connecting to IP address: '192.168.255.10:50240'
[ INFO] [1525802331.457070987]: Loading topic list
[ INFO] [1525802331.457150840]: Found 4 topics
[ INFO] [1525802331.457232638]: Topic(state_value): [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525802331.457297440]: Topic(state_value): [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525802331.457354225]: Topic(state_value): [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525802331.457413602]: Topic(state_value): [group:2,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525802331.457467015]: Loading group: [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525802331.457594219]: Loading group: [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525802331.457662366]: Loading group: [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525802331.457722454]: Loading group: [group:2,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525802331.457795325]: Loaded 3 groups
[ INFO] [1525802331.457857552]: MotomanJointTrajectoryStreamer: init
[ INFO] [1525802331.457904468]: JointTrajectoryStreamer: init
[ INFO] [1525802331.458500373]: Connected to server
[ WARN] [1525802331.459938562]: Unable to read velocity limits from 'robot_description' param.  Velocity validation disabled.
[ INFO] [1525802331.469278550]: Added message handler for message type: 10
[ INFO] [1525802331.471845604]: Added message handler for message type: 15
[ INFO] [1525802331.471925152]: Creating joint_feedback_ex_relay_handler with 3 groups
[ INFO] [1525802331.479876680]: Loading topic list
[ INFO] [1525802331.479983408]: Found 4 topics
[ INFO] [1525802331.480034636]: Topic(state_value): [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525802331.480086592]: Topic(state_value): [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525802331.480130836]: Topic(state_value): [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525802331.480174577]: Topic(state_value): [group:2,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525802331.480214888]: Loading group: [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525802331.480327181]: Loading group: [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525802331.480380941]: Loading group: [group:2,joints:{torso_joint_b1},name:sda20d_b1_controller,ns:sda20d]
[ INFO] [1525802331.480426611]: Loading group: [group:2,joints:{torso_joint_b2},name:sda20d_b2_controller,ns:sda20d]
[ INFO] [1525802331.480475311]: Loaded 3 groups
[ INFO] [1525802331.481578268]: Added message handler for message type: 2017
[ INFO] [1525802331.485001516]: Connected to server
[ INFO] [1525802331.485061847]: Successfully initialized robot state interface
[ INFO] [1525802331.485108232]: Entering message manager spin loop
[ INFO] [1525802331.552928670]: Unlocking mutex
[ INFO] [1525802331.553042195]: Starting Motoman joint trajectory streamer thread
[ INFO] [1525802331.558210887]: Connecting to robot motion server
[ WARN] [1525802331.558278139]: Tried to connect when socket already in connected state
[FATAL] [1525802331.673627559]: ASSERTION FAILED
	file = /home/sda20/cidesi_robots_ws/src/motoman-indigo-devel/motoman_driver/src/industrial_robot_client/joint_relay_handler.cpp
	line = 320
	cond = all_joint_state.positions.size() == all_joint_names.size()

[joint_state-1] process has died [pid 23590, exit code -5, cmd /home/sda20/cidesi_robots_ws/devel/lib/motoman_driver/robot_state __name:=joint_state __log:=/home/sda20/.ros/log/00b5a52a-52e8-11e8-97d3-009a9e9d9347/joint_state-1.log].
log file: /home/sda20/.ros/log/00b5a52a-52e8-11e8-97d3-009a9e9d9347/joint_state-1*.log

[ INFO] [1525807605.449170847]: Added message handler for message type: 0
[ INFO] [1525807605.455824568]: Robot state connecting to IP address: '192.168.255.10:50241'
[ INFO] [1525807605.458944544]: Loading topic list
[ INFO] [1525807605.459047732]: Found 2 topics
[ INFO] [1525807605.459115870]: Topic(state_value): [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525807605.459233539]: Topic(state_value): [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525807605.459333939]: Loading group: [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525807605.459564509]: Loading group: [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525807605.459692435]: Loaded 2 groups
[ INFO] [1525807605.459811539]:  Initializing robot state 2 groups
[ INFO] [1525807605.459909898]: Initializing message manager with default comms fault handler
[ INFO] [1525807605.459998111]: Default communications fault handler successfully initialized
[ INFO] [1525807605.460075912]: Initializing message manager
[ INFO] [1525807605.460160439]: Added message handler for message type: 1
process[joint_trajectory_action-3]: started with pid [25755]
[ INFO] [1525807605.470761572]: Added message handler for message type: 10
[ INFO] [1525807605.472210634]: Added message handler for message type: 15
[ INFO] [1525807605.472312112]: Creating joint_feedback_ex_relay_handler with 2 groups
[ INFO] [1525807605.482048725]: Added message handler for message type: 2017
[ INFO] [1525807605.482837964]: Joint Trajectory Interface connecting to IP address: '192.168.255.10:50240'
[ INFO] [1525807605.485417537]: Connected to server
[ INFO] [1525807605.485497443]: Successfully initialized robot state interface
[ INFO] [1525807605.485588419]: Entering message manager spin loop
[ INFO] [1525807605.490193465]: Loading topic list
[ INFO] [1525807605.490264995]: Found 2 topics
[ INFO] [1525807605.490309897]: Topic(state_value): [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525807605.490350038]: Topic(state_value): [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525807605.490412647]: Loading group: [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525807605.490550847]: Loading group: [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525807605.490629770]: Loaded 2 groups
[ INFO] [1525807605.490700324]: MotomanJointTrajectoryStreamer: init
[ INFO] [1525807605.490748014]: JointTrajectoryStreamer: init
[ INFO] [1525807605.491187031]: Connected to server
[ INFO] [1525807605.500241632]: Loading topic list
[ INFO] [1525807605.500364735]: Found 2 topics
[ INFO] [1525807605.500438567]: Topic(state_value): [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525807605.500509860]: Topic(state_value): [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525807605.500577576]: Loading group: [group:0,joints:{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},name:sda20d_r2_controller,ns:sda20d]
[ INFO] [1525807605.500706123]: Loading group: [group:1,joints:{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},name:sda20d_r1_controller,ns:sda20d]
[ INFO] [1525807605.500785825]: Loaded 2 groups
[ INFO] [1525807605.561350326]: Unlocking mutex
[ INFO] [1525807605.561490191]: Starting Motoman joint trajectory streamer thread
[ INFO] [1525807605.566693825]: Connecting to robot motion server
[ WARN] [1525807605.566751532]: Tried to connect when socket already in connected state
[FATAL] [1525807605.575907532]: ASSERTION FAILED
	file = /opt/ros/indigo/include/ros/publisher.h
	line = 102
	cond = false
	message = 
[FATAL] [1525807605.575987132]: Call to publish() on an invalid Publisher
[FATAL] [1525807605.576013285]: 

[joint_state-1] process has died [pid 25742, exit code -5, cmd /home/sda20/cidesi_robots_ws/devel/lib/motoman_driver/robot_state __name:=joint_state __log:=/home/sda20/.ros/log/00b5a52a-52e8-11e8-97d3-009a9e9d9347/joint_state-1.log].
log file: /home/sda20/.ros/log/00b5a52a-52e8-11e8-97d3-009a9e9d9347/joint_state-1*.log

@Sinai_Aranda (and all) If you like you could try our pre-release version of Rosvita which is a ready-to-use ROS docker container with web UI (see our early access documentation). It comes with models & configurations for SDA10D/F, Universal Robots and Meca500 and is for free for public research institutes and private users. Just send me a PM.

I have resolved the problem. The count is from zero for number of groups just delete ros_assert. This have a while.
In rostopic now already it’s show me all joints in time real. Also, I move the robot (R1, R2, B1) and it’s change the joint values. This load yaml of 4 groups (same file), but do not show values of B2 joint.

So, I will start to do test of moving robot. thanks!!!

1 Like

No, 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. 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.

Probably the hardest change we had to make: In the robot controller configuration we removed the B2 group.

These are the sda10d_motion_interface.yaml and the joint_names_sda10d.yaml we are using for our DX100.

And regarding the MotoPlus driver (@ted-miller): The documentation of mpMeiIncrementMove was quite challenging to decode - I guess the text about MP_POS_DATA.ctrl_grp in the mpManualMOV description also applies to mpMeiIncrementMove. So what I found is: It is possible to control R1 + B1 in one mpMeiIncrementMove command for MP_SL_ID1 by setting the corresponding two bits of the ctrl_grp member. After that it is possible to execute another mpMeiIncrementMove for MP_SL_ID2 with the ctrl_grp mask with a bit set for the data for R2. Both calls being made in one interpolation period results in smooth whole body motions.

That is not a solution. The assert is there for a reason, and you’re essentially running into motoman/#91.

Looking at the output in your previous post, it appears you’ve duplicated group 2, that won’t work. Your robot has either 2 or 3 groups, and the motion_interface.yaml should reflect that.

The configuration that @andreaskoepf posted would be more like what you should be using, but then we’d have to fix motoman/#91.

If @ted-miller could clarify whether the approach used by @andreaskoepf is acceptable / supported, you could even use that configuration and the MotoPlus changes. But let’s wait for his comments.

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.

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(..)?

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?