Use Moveit with YuMI

[yanjunyangmonash]

Hi @gavanderhoorn, I followed your discussion here to set up the moveit package. The URDF file I use is from this repository which follows the non-standard joints order. When I ran the moveit_planning_execution.launch, The robot in RVIZ didn't match the pose of the robot shown in the robotstudio.

I guess the problem is from the published joint states, the name order in the message mismatch the order defined in URDF file.

header: 
  seq: 91
  stamp: 
    secs: 1614610434
    nsecs: 466701072
  frame_id: ''
name: [yumi_robr_joint_1, yumi_robr_joint_2, yumi_robr_joint_3, yumi_robr_joint_4, yumi_robr_joint_5, yumi_robr_joint_6, yumi_robr_joint_7, yumi_robl_joint_1, yumi_robl_joint_2, yumi_robl_joint_3, yumi_robl_joint_4, yumi_robl_joint_5, yumi_robl_joint_6, yumi_robl_joint_7]
position: [0.0, -2.2689280275926285, -2.356194490192345, 0.5235987755982988, 0.0, 0.6981317007977318, 1.7303211408292425, 0.0, -2.2689280275926285, 2.356194490192345, 0.5235987755982988, 0.0, 0.6981317007977318, -1.7306115585461157]

and the correct order should be

[yumi_robr_joint_1, yumi_robr_joint_2, yumi_robr_joint_7, yumi_robr_joint_3, yumi_robr_joint_4, yumi_robr_joint_5, yumi_robr_joint_6, yumi_robl_joint_1, yumi_robl_joint_2, yumi_robl_joint_7, yumi_robl_joint_3, yumi_robl_joint_4, yumi_robl_joint_5, yumi_robl_joint_6]

I saw your reply to this question and understand there is no way to sort the joint order in the message. I also tried this method but it didn't work.

I'm wondering if there is a good way to keep the joint name match the published data for RVIZ to use?

[gavanderhoorn]

and the correct order should be

As I was trying to explain in the ROS Answers Q&A you link, there is no correct order -- or more precisely: there is no incorrect order.

Consumers of JointState messages are expected to treat a message like a Python dict, with dict[str, float] as the type. Keys would be elements in the name field, values the elements in position. Lookups for joints would then be based on name, not on index.

Pseudo-code:

states = dict(zip(msg.name, msg.position))
right_joint2_pos = states['yumi_robr_joint_2']

I'm almost certain what you report is not because of any incorrect order, as the RobotModel display in RViz does not care about JointState messages. It uses TF, which is broadcast by the robot_state_publisher. And the RSP does what I wrote above (not exactly, but basically the same).

We're going to need more information in order to help you, as

the robot in RVIZ didn't match the pose of the robot shown in the robotstudio.

is not enough.

[yanjunyangmonash]

@gavanderhoorn I probably didn't make it clear in my first post. It's not about the name order, but the mapping from the name to position in the message. In position, the joint is given in the order joint 1,2,7,3,4,5,6 and doesn't match the above name.

From the pedant, the value for the right arm is

and because of the mismatch problem (I doubt), the robot in RIVZ looked like this

Besides, what else info do you need? Do I need to upload my moveit_planning_execution.launch, ex3_controllers.yaml and moveit_planning_execution.launch? These are all files that I modified

[gavanderhoorn]

Could you please run abb_robot_bringup_examples/launch/ex3_rws_and_egm_yumi_robot.launch with debug:=true (and perhaps even with --screen appended to the roslaunch args)?

And of course make sure to update whatever parameters needed for your specific setup.

Then post the complete output here in a code block.

---

Edit: also: what is the Global Status error?

---

Edit 2: ah, I think I may know what's going wrong. You/we're confusing the joint value at index 3 (which is how the ABB controller internally maps things) to what we see/use on the ROS side.

As soon as we're working with JointStates or URDFs, we don't care any more about what the controller does. The abb_robot_driver node does the right thing, and converts whatever internal representation is used to a "normal one" (ie: the 7th joint is in the 7th location, as you'd expect).

joint_7 is the seventh joint in the kinematic chain of each arm, not the third one.

I believe @robberthofmanfm changed the order of joints in this commit in his fork, but that is most likely not needed -- or it should not be needed.

It would still be good to report the output of the .launch file though, just so we can make sure.

[yanjunyangmonash]

The output below is running with ex3_rws_and_egm_yumi_robot.launch using the default parameters. There are mismatch problems in standardized_joints.

Click to expand

``` SUMMARY ======== PARAMETERS * /rosdistro: melodic * /rosversion: 1.14.10 * /yumi/egm/egm_state_controller/publish_rate: 250 * /yumi/egm/egm_state_controller/type: abb_egm_state_con... * /yumi/egm/joint_group_velocity_controller/joints: ['yumi_robl_joint... * /yumi/egm/joint_group_velocity_controller/type: velocity_controll... * /yumi/egm/joint_position_trajectory_controller/joints: ['yumi_robl_joint... * /yumi/egm/joint_position_trajectory_controller/type: position_controll... * /yumi/egm/joint_state_controller/publish_rate: 250 * /yumi/egm/joint_state_controller/type: joint_state_contr... * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_1/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_1/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_1/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_1/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_2/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_2/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_2/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_2/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_3/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_3/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_3/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_3/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_4/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_4/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_4/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_4/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_5/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_5/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_5/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_5/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_6/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_6/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_6/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_6/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_7/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_7/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_7/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robl_joint_7/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_1/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_1/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_1/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_1/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_2/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_2/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_2/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_2/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_3/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_3/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_3/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_3/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_4/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_4/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_4/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_4/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_5/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_5/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_5/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_5/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_6/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_6/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_6/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_6/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_7/d: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_7/i: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_7/i_clamp: 1 * /yumi/egm/joint_velocity_trajectory_controller/gains/yumi_robr_joint_7/p: 100 * /yumi/egm/joint_velocity_trajectory_controller/joints: ['yumi_robl_joint... * /yumi/egm/joint_velocity_trajectory_controller/type: velocity_controll... * /yumi/egm_controller_stopper/no_service_timeout: False * /yumi/egm_controller_stopper/ros_control/controllers/always_ok_to_start: ['egm_state_contr... * /yumi/egm_controller_stopper/ros_control/controllers/ok_to_keep_running: ['egm_state_contr... * /yumi/egm_hardware_interface/egm/channel_1/mech_unit_group: rob_l * /yumi/egm_hardware_interface/egm/channel_1/port_number: 6511 * /yumi/egm_hardware_interface/egm/channel_2/mech_unit_group: rob_r * /yumi/egm_hardware_interface/egm/channel_2/port_number: 6512 * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_1/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_1/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_2/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_2/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_3/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_3/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_4/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_4/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_5/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_5/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_6/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_6/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_7/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robl_joint_7/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_1/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_1/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_2/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_2/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_3/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_3/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_4/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_4/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_5/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_5/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_6/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_6/max_velocity: 0.2 * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_7/has_velocity_limits: True * /yumi/egm_hardware_interface/joint_limits/yumi_robr_joint_7/max_velocity: 0.2 * /yumi/egm_hardware_interface/no_service_timeout: False * /yumi/egm_hardware_interface/ros_control/controllers/always_ok_to_start: ['egm_state_contr... * /yumi/egm_hardware_interface/ros_control/controllers/ok_to_keep_running: ['egm_state_contr... * /yumi/rws_service_provider/no_connection_timeout: False * /yumi/rws_service_provider/robot_ip: 192.168.0.55 * /yumi/rws_service_provider/robot_nickname: yumi * /yumi/rws_service_provider/robot_port: 80 * /yumi/rws_state_publisher/no_connection_timeout: False * /yumi/rws_state_publisher/polling_rate: 5 * /yumi/rws_state_publisher/robot_ip: 192.168.0.55 * /yumi/rws_state_publisher/robot_nickname: yumi * /yumi/rws_state_publisher/robot_port: 80 NODES /yumi/ egm_controller_stopper (abb_egm_hardware_interface/egm_controller_stopper) egm_hardware_interface (abb_egm_hardware_interface/egm_hardware_interface) rws_service_provider (abb_rws_service_provider/rws_service_provider) rws_state_publisher (abb_rws_state_publisher/rws_state_publisher) /yumi/egm/ started (controller_manager/spawner) stopped (controller_manager/spawner) auto-starting new master process[master]: started with pid [2833] ROS_MASTER_URI=http://localhost:11311 setting /run_id to 6e9926d2-7aab-11eb-a515-000c29d23b0d process[rosout-1]: started with pid [2844] started core service [/rosout] process[yumi/rws_state_publisher-2]: started with pid [2851] process[yumi/rws_service_provider-3]: started with pid [2852] process[yumi/egm_hardware_interface-4]: started with pid [2853] [DEBUG] [1614616228.654920657]: Get ROS parameters (namespace '/yumi/rws_state_publisher') [DEBUG] [1614616228.655614511]: Check for (optional) parameter 'robot_ip' [DEBUG] [1614616228.656555995]: Found parameter 'robot_ip'='192.168.0.55' [DEBUG] [1614616228.656601785]: Check for (optional) parameter 'robot_port' [DEBUG] [1614616228.659537513]: Found parameter 'robot_port'='80' [DEBUG] [1614616228.659610210]: Check for (optional) parameter 'robot_nickname' process[yumi/egm_controller_stopper-5]: started with pid [2859] [DEBUG] [1614616228.662343709]: Get ROS parameters (namespace '/yumi/rws_service_provider') [DEBUG] [1614616228.662807952]: Found parameter 'robot_nickname'='yumi' [DEBUG] [1614616228.662849417]: Check for (optional) parameter 'no_connection_timeout' [DEBUG] [1614616228.665142960]: Found parameter 'no_connection_timeout'='0' [DEBUG] [1614616228.665183526]: Check for (optional) parameter 'polling_rate' [DEBUG] [1614616228.666488097]: Found parameter 'polling_rate'='5' [ INFO] [1614616228.666566974]: Initializing... [DEBUG] [1614616228.667490105]: Check for (optional) parameter 'robot_ip' [DEBUG] [1614616228.668908842]: Check for (optional) parameter 'no_service_timeout' process[yumi/egm/started-6]: started with pid [2866] [DEBUG] [1614616228.670126941]: Found parameter 'no_service_timeout'='0' [DEBUG] [1614616228.670162024]: Wait for ROS service 'rws/get_robot_controller_description' [ INFO] [1614616228.670991917]: waitForService: Service [/yumi/rws/get_robot_controller_description] has not been advertised, waiting... [DEBUG] [1614616228.671905171]: Found parameter 'robot_ip'='192.168.0.55' [DEBUG] [1614616228.671970804]: Check for (optional) parameter 'robot_port' [DEBUG] [1614616228.673464449]: Found parameter 'robot_port'='80' [DEBUG] [1614616228.673510429]: Check for (optional) parameter 'robot_nickname' [DEBUG] [1614616228.673940823]: Found parameter 'robot_nickname'='yumi' [DEBUG] [1614616228.673992128]: Check for (optional) parameter 'no_connection_timeout' [DEBUG] [1614616228.674894015]: Get ROS parameters (namespace '/yumi/egm_controller_stopper') [DEBUG] [1614616228.675061083]: Found parameter 'no_connection_timeout'='0' [ INFO] [1614616228.675135773]: Initializing... process[yumi/egm/stopped-7]: started with pid [2879] [DEBUG] [1614616228.678846431]: Check for (optional) parameter 'no_service_timeout' [DEBUG] [1614616228.679667446]: Found parameter 'no_service_timeout'='0' [DEBUG] [1614616228.679715369]: Check for (optional) parameter 'ros_control/controllers/ok_to_keep_running' [DEBUG] [1614616228.680554987]: Found parameter 'ros_control/controllers/ok_to_keep_running'='[egm_state_controller, joint_state_controller]' [ INFO] [1614616228.683584488]: Initializing... [ INFO] [1614616228.683991697]: waitForService: Service [/yumi/egm/controller_manager/list_controllers] has not been advertised, waiting... [DEBUG] [1614616228.987244096]: Robot controller description: ============================================================ = Summary of robot controller at '192.168.0.55:80' ============================================================ # General Information: |- RobotWare version: 6.11.03.00 |- System name: StateMachineSystem |- System type: Virtual Controller |- Options: |- UDPUC Driver |- Contact L |- Leadthrough |- YuMi AbsAcc Recovery |- English |- 613-1 Collision Detection |- 709-1 DeviceNet Master/Slave |- 689-1 Externally Guided Motion (EGM) |- 604-1 MultiMove Coordinated |- 623-1 Multitasking |- 616-1 PC Interface |- RobotWare Base |- Hall Sensor Calibration |- Drive System IRB 14000 |- IRB 14000-0.5/0.5 |- IRB 14000-0.5/0.5 |- IRB 14000-0.5/0.5 |- StateMachine Core # Mechanical Units: |- Unit: ROB_R |- Unit: ROB_R_7 |- Unit: ROB_L |- Unit: ROB_L_7 # Mechanical Unit Groups: |- Group: rob_r |- Group: rob_l ============================================================ [DEBUG] [1614616228.989714578]: Robot controller description: ============================================================ = Summary of robot controller at '192.168.0.55:80' ============================================================ # General Information: |- RobotWare version: 6.11.03.00 |- System name: StateMachineSystem |- System type: Virtual Controller |- Options: |- UDPUC Driver |- Contact L |- Leadthrough |- YuMi AbsAcc Recovery |- English |- 613-1 Collision Detection |- 709-1 DeviceNet Master/Slave |- 689-1 Externally Guided Motion (EGM) |- 604-1 MultiMove Coordinated |- 623-1 Multitasking |- 616-1 PC Interface |- RobotWare Base |- Hall Sensor Calibration |- Drive System IRB 14000 |- IRB 14000-0.5/0.5 |- IRB 14000-0.5/0.5 |- IRB 14000-0.5/0.5 |- StateMachine Core # Mechanical Units: |- Unit: ROB_R |- Unit: ROB_R_7 |- Unit: ROB_L |- Unit: ROB_L_7 # Mechanical Unit Groups: |- Group: rob_r |- Group: rob_l ============================================================ [ INFO] [1614616228.990816393]: Initialization succeeded, and the node is ready for use [INFO] [1614616228.993589]: Controller Spawner: Waiting for service controller_manager/load_controller [DEBUG] [1614616228.996973924]: Adding basic services [ INFO] [1614616229.001946243]: waitForService: Service [/yumi/rws/get_robot_controller_description] is now available. [DEBUG] [1614616229.007354634]: StateMachine Add-In 1.1 detected, adding additional services [ INFO] [1614616229.010278387]: Initialization succeeded, and the node is ready for use [ INFO] [1614616229.012306545]: Initializing... [DEBUG] [1614616229.012348732]: Check for (mandatory) parameter 'robot_controller_description' [DEBUG] [1614616229.012902889]: Found parameter 'robot_controller_description'='header { ip_address: "192.168.0.55" rws_port_number: 80 robot_ware_version { name: "6.11.03.00" major_number: 6 minor_number: 11 patch_number: 3 } system_name: "StateMachineSystem" system_type: "Virtual Controller" options: "UDPUC Driver" options: "Contact L" options: "Leadthrough" options: "YuMi AbsAcc Recovery" options: "English" options: "613-1 Collision Detection" options: "709-1 DeviceNet Master/Slave" options: "689-1 Externally Guided Motion (EGM)" options: "604-1 MultiMove Coordinated" options: "623-1 Multitasking" options: "616-1 PC Interface" options: "RobotWare Base" options: "Hall Sensor Calibration" options: "Drive System IRB 14000" options: "IRB 14000-0.5/0.5" options: "IRB 14000-0.5/0.5" options: "IRB 14000-0.5/0.5" options: "StateMachine Core" } system_indicators { robots { irb14000: true } options { egm: true leadthrough: true multimove: true } addins { state_machine_1_1: true } } mechanical_units_groups { name: "rob_r" robot { name: "ROB_R" robot { name: "ROB_R" type: "ROB_R_14000_05_05" joints { name: "rob_R_1" logical_axis: 1 kinematic_axis_number: 1 arm { name: "rob_R_1" lower_joint_bound: -2.9408801 upper_joint_bound: 2.9408801 } transmission { name: "rob_R_1" rotating_move: true } } joints { name: "rob_R_2" logical_axis: 2 kinematic_axis_number: 2 arm { name: "rob_R_2" lower_joint_bound: -2.50455 upper_joint_bound: 0.75921798 } transmission { name: "rob_R_2" rotating_move: true } } joints { name: "rob_R_3" logical_axis: 3 kinematic_axis_number: 4 arm { name: "rob_R_3" lower_joint_bound: -2.1554799 upper_joint_bound: 1.39626 } transmission { name: "rob_R_3" rotating_move: true } } joints { name: "rob_R_4" logical_axis: 4 kinematic_axis_number: 5 arm { name: "rob_R_4" lower_joint_bound: -5.06145 upper_joint_bound: 5.06145 } transmission { name: "rob_R_4" rotating_move: true } } joints { name: "rob_R_5" logical_axis: 5 kinematic_axis_number: 6 arm { name: "rob_R_5" lower_joint_bound: -1.53589 upper_joint_bound: 2.40855 } transmission { name: "rob_R_5" rotating_move: true } } joints { name: "rob_R_6" logical_axis: 6 kinematic_axis_number: 7 arm { name: "rob_R_6" lower_joint_bound: -3.9967999 upper_joint_bound: 3.9967999 } transmission { name: "rob_R_6" rotating_move: true } } base_frame { position { x: 0 y: 0 z: 0 } rotation { q1: 1 q2: 0 q3: 0 q4: 0 } } base_frame_moved_by: "" } type: TCP_ROBOT task_name: "T_ROB_R" axes: 6 axes_total: 7 is_integrated_unit: "NoIntegratedUnit" has_integrated_unit: "ROB_R_7" status: "Synchronized" mode: ACTIVATED standardized_joints { original_name: "rob_R_1" standardized_name: "yumi_robr_joint_1" rotating_move: true lower_joint_bound: -2.9408801 upper_joint_bound: 2.9408801 } standardized_joints { original_name: "rob_R_2" standardized_name: "yumi_robr_joint_2" rotating_move: true lower_joint_bound: -2.50455 upper_joint_bound: 0.75921798 } standardized_joints { original_name: "rob_R_7" standardized_name: "yumi_robr_joint_3" rotating_move: true lower_joint_bound: -2.9408801 upper_joint_bound: 2.9408801 } standardized_joints { original_name: "rob_R_3" standardized_name: "yumi_robr_joint_4" rotating_move: true lower_joint_bound: -2.1554799 upper_joint_bound: 1.39626 } standardized_joints { original_name: "rob_R_4" standardized_name: "yumi_robr_joint_5" rotating_move: true lower_joint_bound: -5.06145 upper_joint_bound: 5.06145 } standardized_joints { original_name: "rob_R_5" standardized_name: "yumi_robr_joint_6" rotating_move: true lower_joint_bound: -1.53589 upper_joint_bound: 2.40855 } standardized_joints { original_name: "rob_R_6" standardized_name: "yumi_robr_joint_7" rotating_move: true lower_joint_bound: -3.9967999 upper_joint_bound: 3.9967999 } } mechanical_units { name: "ROB_R_7" singles { name: "ROB_R_7" type: "ROB_R_7" joint { name: "rob_R_7" logical_axis: 7 kinematic_axis_number: 3 arm { name: "rob_R_7" lower_joint_bound: -2.9408801 upper_joint_bound: 2.9408801 } transmission { name: "rob_R_7" rotating_move: true } } base_frame { position { x: 0 y: 0 z: 0 } rotation { q1: 1 q2: 0 q3: 0 q4: 0 } } base_frame_moved_by: "" } type: SINGLE task_name: "T_ROB_R" axes: 1 axes_total: 1 is_integrated_unit: "ROB_R" has_integrated_unit: "NoIntegratedUnit" status: "Synchronized" mode: ACTIVATED } } mechanical_units_groups { name: "rob_l" robot { name: "ROB_L" robot { name: "ROB_L" type: "ROB_L_14000_05_05" joints { name: "rob_L_1" logical_axis: 1 kinematic_axis_number: 1 arm { name: "rob_L_1" lower_joint_bound: -2.9408801 upper_joint_bound: 2.9408801 } transmission { name: "rob_L_1" rotating_move: true } } joints { name: "rob_L_2" logical_axis: 2 kinematic_axis_number: 2 arm { name: "rob_L_2" lower_joint_bound: -2.50455 upper_joint_bound: 0.75921798 } transmission { name: "rob_L_2" rotating_move: true } } joints { name: "rob_L_3" logical_axis: 3 kinematic_axis_number: 4 arm { name: "rob_L_3" lower_joint_bound: -2.1554799 upper_joint_bound: 1.39626 } transmission { name: "rob_L_3" rotating_move: true } } joints { name: "rob_L_4" logical_axis: 4 kinematic_axis_number: 5 arm { name: "rob_L_4" lower_joint_bound: -5.06145 upper_joint_bound: 5.06145 } transmission { name: "rob_L_4" rotating_move: true } } joints { name: "rob_L_5" logical_axis: 5 kinematic_axis_number: 6 arm { name: "rob_L_5" lower_joint_bound: -1.53589 upper_joint_bound: 2.40855 } transmission { name: "rob_L_5" rotating_move: true } } joints { name: "rob_L_6" logical_axis: 6 kinematic_axis_number: 7 arm { name: "rob_L_6" lower_joint_bound: -3.9967999 upper_joint_bound: 3.9967999 } transmission { name: "rob_L_6" rotating_move: true } } base_frame { position { x: 0 y: 0 z: 0 } rotation { q1: 1 q2: 0 q3: 0 q4: 0 } } base_frame_moved_by: "" } type: TCP_ROBOT task_name: "T_ROB_L" axes: 6 axes_total: 7 is_integrated_unit: "NoIntegratedUnit" has_integrated_unit: "ROB_L_7" status: "Synchronized" mode: ACTIVATED standardized_joints { original_name: "rob_L_1" standardized_name: "yumi_robl_joint_1" rotating_move: true lower_joint_bound: -2.9408801 upper_joint_bound: 2.9408801 } standardized_joints { original_name: "rob_L_2" standardized_name: "yumi_robl_joint_2" rotating_move: true lower_joint_bound: -2.50455 upper_joint_bound: 0.75921798 } standardized_joints { original_name: "rob_L_7" standardized_name: "yumi_robl_joint_3" rotating_move: true lower_joint_bound: -2.9408801 upper_joint_bound: 2.9408801 } standardized_joints { original_name: "rob_L_3" standardized_name: "yumi_robl_joint_4" rotating_move: true lower_joint_bound: -2.1554799 upper_joint_bound: 1.39626 } standardized_joints { original_name: "rob_L_4" standardized_name: "yumi_robl_joint_5" rotating_move: true lower_joint_bound: -5.06145 upper_joint_bound: 5.06145 } standardized_joints { original_name: "rob_L_5" standardized_name: "yumi_robl_joint_6" rotating_move: true lower_joint_bound: -1.53589 upper_joint_bound: 2.40855 } standardized_joints { original_name: "rob_L_6" standardized_name: "yumi_robl_joint_7" rotating_move: true lower_joint_bound: -3.9967999 upper_joint_bound: 3.9967999 } } mechanical_units { name: "ROB_L_7" singles { name: "ROB_L_7" type: "ROB_L_7" joint { name: "rob_L_7" logical_axis: 7 kinematic_axis_number: 3 arm { name: "rob_L_7" lower_joint_bound: -2.9408801 upper_joint_bound: 2.9408801 } transmission { name: "rob_L_7" rotating_move: true } } base_frame { position { x: 0 y: 0 z: 0 } rotation { q1: 1 q2: 0 q3: 0 q4: 0 } } base_frame_moved_by: "" } type: SINGLE task_name: "T_ROB_L" axes: 1 axes_total: 1 is_integrated_unit: "ROB_L" has_integrated_unit: "NoIntegratedUnit" status: "Synchronized" mode: ACTIVATED } } rapid_tasks { name: "T_ROB_R" is_motion_task: true is_active: true execution_state: STOPPED modules { name: "TRobRAPID" type: "ProgMod" } modules { name: "TRobSystem" type: "SysMod" } modules { name: "user" type: "SysMod" } modules { name: "BASE" type: "SysMod" } modules { name: "TRobEGM" type: "ProgMod" } modules { name: "TRobSystemExample" type: "SysMod" } modules { name: "TRobMain" type: "ProgMod" } modules { name: "TRobUtility" type: "ProgMod" } } rapid_tasks { name: "T_ROB_L" is_motion_task: true is_active: true execution_state: STOPPED modules { name: "TRobRAPID" type: "ProgMod" } modules { name: "TRobSystem" type: "SysMod" } modules { name: "user" type: "SysMod" } modules { name: "BASE" type: "SysMod" } modules { name: "TRobEGM" type: "ProgMod" } modules { name: "TRobSystemExample" type: "SysMod" } modules { name: "TRobMain" type: "ProgMod" } modules { name: "TRobUtility" type: "ProgMod" } } ' [DEBUG] [1614616229.014125074]: Robot controller description: ============================================================ = Summary of robot controller at '192.168.0.55:80' ============================================================ # General Information: |- RobotWare version: 6.11.03.00 |- System name: StateMachineSystem |- System type: Virtual Controller |- Options: |- UDPUC Driver |- Contact L |- Leadthrough |- YuMi AbsAcc Recovery |- English |- 613-1 Collision Detection |- 709-1 DeviceNet Master/Slave |- 689-1 Externally Guided Motion (EGM) |- 604-1 MultiMove Coordinated |- 623-1 Multitasking |- 616-1 PC Interface |- RobotWare Base |- Hall Sensor Calibration |- Drive System IRB 14000 |- IRB 14000-0.5/0.5 |- IRB 14000-0.5/0.5 |- IRB 14000-0.5/0.5 |- StateMachine Core # Mechanical Units: |- Unit: ROB_R |- Unit: ROB_R_7 |- Unit: ROB_L |- Unit: ROB_L_7 # Mechanical Unit Groups: |- Group: rob_r |- Group: rob_l ============================================================ [DEBUG] [1614616229.014213100]: Get ROS parameters (namespace: '/yumi/egm_hardware_interface/egm') [DEBUG] [1614616229.014233826]: Check for 'channel_1' parameter [DEBUG] [1614616229.015410702]: Found 'channel_1' parameter [DEBUG] [1614616229.015467884]: Check for (optional) parameter 'egm/channel_1/mech_unit_group' [DEBUG] [1614616229.016149707]: Found parameter 'egm/channel_1/mech_unit_group'='rob_l' [DEBUG] [1614616229.016222026]: Check for (mandatory) parameter 'egm/channel_1/port_number' [DEBUG] [1614616229.016661273]: Found parameter 'egm/channel_1/port_number'='6511' [DEBUG] [1614616229.016710387]: Port number parameter '6511' found [DEBUG] [1614616229.016726195]: Check for 'channel_2' parameter [DEBUG] [1614616229.017229420]: Found 'channel_2' parameter [DEBUG] [1614616229.017282368]: Check for (optional) parameter 'egm/channel_2/mech_unit_group' [DEBUG] [1614616229.018054478]: Found parameter 'egm/channel_2/mech_unit_group'='rob_r' [DEBUG] [1614616229.018105106]: Check for (mandatory) parameter 'egm/channel_2/port_number' [DEBUG] [1614616229.018582211]: Found parameter 'egm/channel_2/port_number'='6512' [DEBUG] [1614616229.018631680]: Port number parameter '6512' found [DEBUG] [1614616229.018646807]: Check for 'channel_3' parameter [DEBUG] [1614616229.019100705]: Initializing EGM manager [DEBUG] [1614616229.019152524]: Verifying ROS parameters against the RWS robot controller description [DEBUG] [1614616229.019232821]: Verified '2' EGM channel configuration(s) against the RWS robot controller description [DEBUG] [1614616229.019600624]: Check for (optional) parameter 'ros_control/controllers/always_ok_to_start' [DEBUG] [1614616229.020648453]: Found parameter 'ros_control/controllers/always_ok_to_start'='[egm_state_controller, joint_state_controller]' [ WARN] [1614616229.025677409]: Configured default soft limits for 'yumi_robr_joint_1': position [-2.91147; 2.91147], k: 1 [ WARN] [1614616229.029176090]: Configured default soft limits for 'yumi_robr_joint_2': position [-2.4795; 0.751626], k: 1 [ WARN] [1614616229.033149706]: Configured default soft limits for 'yumi_robr_joint_3': position [-2.91147; 2.91147], k: 1 [ WARN] [1614616229.036824925]: Configured default soft limits for 'yumi_robr_joint_4': position [-2.13393; 1.3823], k: 1 [ WARN] [1614616229.039883593]: Configured default soft limits for 'yumi_robr_joint_5': position [-5.01084; 5.01084], k: 1 [ WARN] [1614616229.043374950]: Configured default soft limits for 'yumi_robr_joint_6': position [-1.52053; 2.38446], k: 1 [ WARN] [1614616229.047251510]: Configured default soft limits for 'yumi_robr_joint_7': position [-3.95683; 3.95683], k: 1 [ WARN] [1614616229.050440404]: Configured default soft limits for 'yumi_robl_joint_1': position [-2.91147; 2.91147], k: 1 [ WARN] [1614616229.053743285]: Configured default soft limits for 'yumi_robl_joint_2': position [-2.4795; 0.751626], k: 1 [ WARN] [1614616229.056734907]: Configured default soft limits for 'yumi_robl_joint_3': position [-2.91147; 2.91147], k: 1 [ WARN] [1614616229.060592637]: Configured default soft limits for 'yumi_robl_joint_4': position [-2.13393; 1.3823], k: 1 [ WARN] [1614616229.064493245]: Configured default soft limits for 'yumi_robl_joint_5': position [-5.01084; 5.01084], k: 1 [ WARN] [1614616229.067435998]: Configured default soft limits for 'yumi_robl_joint_6': position [-1.52053; 2.38446], k: 1 [ WARN] [1614616229.070995191]: Configured default soft limits for 'yumi_robl_joint_7': position [-3.95683; 3.95683], k: 1 [ INFO] [1614616229.071086677]: Initialization succeeded, and the node is ready for use [INFO] [1614616229.092674]: Controller Spawner: Waiting for service controller_manager/load_controller [INFO] [1614616229.095405]: Controller Spawner: Waiting for service controller_manager/switch_controller [INFO] [1614616229.097242]: Controller Spawner: Waiting for service controller_manager/unload_controller [INFO] [1614616229.099036]: Loading controller: joint_group_velocity_controller [ INFO] [1614616229.104008143]: waitForService: Service [/yumi/egm/controller_manager/list_controllers] is now available. [ INFO] [1614616229.104468399]: Initialization succeeded, and the node is ready for use [INFO] [1614616229.108686]: Controller Spawner: Loaded controllers: joint_group_velocity_controller [INFO] [1614616229.298619]: Controller Spawner: Waiting for service controller_manager/switch_controller [INFO] [1614616229.301790]: Controller Spawner: Waiting for service controller_manager/unload_controller [INFO] [1614616229.304072]: Loading controller: egm_state_controller [DEBUG] [1614616229.308028912]: Check for (optional) parameter 'publish_rate' [DEBUG] [1614616229.308604361]: Found parameter 'publish_rate'='250' [INFO] [1614616229.347622]: Loading controller: joint_state_controller [INFO] [1614616229.397656]: Controller Spawner: Loaded controllers: egm_state_controller, joint_state_controller [ WARN] [1614616229.447331393]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [INFO] [1614616229.448553]: Started controllers: egm_state_controller, joint_state_controller [ WARN] [1614616239.473463225]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1614616249.479388508]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1614616259.494818493]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1614616269.533046412]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1614616279.553165908]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ INFO] [1614616285.314560518]: EGM session state changed: ros_control controllers will be reset [ INFO] [1614616285.339179074]: Controller has resumed EGM feedback [ INFO] [1614616296.856430594]: EGM session state changed: ros_control controllers will be reset [ INFO] [1614616345.878285079]: EGM session state changed: ros_control controllers will be reset [ WARN] [1614616348.271562105]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1614616348.324430500]: EGM became inactive, stopping controllers not allowed to keep running: [joint_group_velocity_controller] [ WARN] [1614616358.278322143]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) ^C[yumi/egm/stopped-7] killing on exit [yumi/egm/started-6] killing on exit [INFO] [1614616364.955807]: Shutting down spawner. Stopping and unloading controllers... [INFO] [1614616364.956964]: Stopping all controllers... [ERROR] [1614616364.959605880]: Could not stop controller 'joint_group_velocity_controller' since it is not running [INFO] [1614616364.959810]: Unloading all loaded controllers... [INFO] [1614616364.960993]: Trying to unload joint_group_velocity_controller [yumi/egm_hardware_interface-4] killing on exit [yumi/egm_controller_stopper-5] killing on exit [yumi/rws_state_publisher-2] killing on exit [yumi/rws_service_provider-3] killing on exit [INFO] [1614616364.968234]: Shutting down spawner. Stopping and unloading controllers... [INFO] [1614616364.969455]: Stopping all controllers... [WARN] [1614616364.991742]: Controller Spawner error while taking down controllers: unable to connect to service: connection from sender terminated before handshake header received. 0 bytes were received. Please check sender for additional details. [WARN] [1614616364.992110]: Controller Spawner error while taking down controllers: unable to connect to service: [Errno 104] Connection reset by peer [rosout-1] killing on exit [master] killing on exit shutting down processing monitor... ... shutting down processing monitor complete done ```

[robberthofmanfm]

Edit 2: ah, I think I may know what's going wrong. You/we're confusing the joint value at index 3 (which is how the ABB controller internally maps things) to what we see/use on the ROS side.

As soon as we're working with JointStates or URDFs, we don't care any more about what the controller does. The abb_robot_driver node does the right thing, and converts whatever internal representation is used to a "normal one" (ie: the 7th joint is in the 7th location, as you'd expect).

joint_7 is the seventh joint in the kinematic chain of each arm, not the third one.

I believe @robberthofmanfm changed the order of joints in this commit in his fork, but that is most likely not needed -- or it should not be needed.

Indeed, the configuration in that commit did not work correctly. In the meantime, we modified the urdf itself because xacro was producing very weird results. https://github.com/robberthofmanfm/yumi/blob/egm_modifications/yumi_description/urdf/yumi.urdf works for us - at least for representing the current joint states of the robot.

[gavanderhoorn]

There are mismatch problems in standardized_joints.

what makes you conclude this?

As I mentioned in https://github.com/ros-industrial/abb_robot_driver/issues/15#issuecomment-788079236: on the ROS side we don't care about what the ABB controller internally does. The 7th joint is the joint in the 7th position of the kinematic chain for us.

For the ABB controller, that would be joint_6. But again, that's not important "outside" the driver.

And I believe the driver does what it's supposed to:

standardized_joints {
  original_name: "rob_R_7"
  standardized_name: "yumi_robr_joint_3"
  [..]
}
[..]
standardized_joints {
  original_name: "rob_L_6"
  standardized_name: "yumi_robl_joint_7"
  rotating_move: true
  lower_joint_bound: -3.9967999
  upper_joint_bound: 3.9967999
}

notice how it's re-named things such that rob_R_7 is now the 3rd joint, while rob_R_6 is now the 7th joint.

As I wrote in https://github.com/ros-industrial/abb_robot_driver/issues/15#issuecomment-788079236: I believe the urdf/xacro you are using has been edited incorrectly by @robberthofmanfm: yumi_robl_joint_7 should link yumi_link_6_r and yumi_link_7_r, but instead yumi_robr_joint_6 does that.

[gavanderhoorn]

@robberthofmanfm wrote:

we modified the urdf itself because xacro was producing very weird results. https://github.com/robberthofmanfm/yumi/blob/egm_modifications/yumi_description/urdf/yumi.urdf works for us - at least for representing the current joint states of the robot.

as long as you're aware plain URDF cannot be combined with any other URDFs.

It would be better to use xacro.

[gavanderhoorn]

@yanjunyangmonash: I'm going to close this issue assuming your problem has been solved -- or at least: we've identified the root cause.

It would seem @robberthofmanfm has already fixed his fork, so the incorrect order of joints in his .urdf should no longer be a problem.

As I don't believe abb_robot_driver does anything wrong, there is nothing for us to fix here.

Please feel free to keep commenting on the issue of course.

[pferfer]

Hello @gavanderhoorn. I followed your tutorial about the use of the abb_robot_driver library in order to simulate the movement of a 6-joint robot (IRB2400) with Robotstudio. I followed all the installation points as well as applied a previous simulation of the abb_driver library with Robotstudio and moveit, which worked. I installed the ABB Server as found in http://wiki.ros.org/abb_driver/Tutorials/InstallServer in order to control the FlexPendant from RobotStudio.

Now, when I try to run your second example I get this error:

[FATAL] [1622463556.209463331]: Runtime error: 'Failed to call ROS service 'rws/get_robot_controller_description''

The controller IP is set as default and I think that it is not a problem about the connection between my ROS and the RobotStudio. Do you know where could be the problem?

Thank you very much.

[gavanderhoorn]

@pferfer: please post new problems in new issues.

This is not a forum, but an issue tracker.