This page describes the joint-remapping functionality provided in the industrial_robot_client library, to support differences between ROS and robot-controller joint ordering.

Motivation

The ROS-Industrial specification uses trajectory_msgs/JointTrajectory and sensor_msgs/JointState messages to communicate joint-position data with other ROS nodes. These message types use an array to specify the ordering of joint data in each individual message packet. No guarantee is made that the joint ordering will be consistent between nodes or individual messages; only that a single message is internally consistent.

By contrast, the simple_message protocol used for communication with the robot controller expects the joints to appear in a pre-defined (robot-specific) ordering.

The joint-remapping capability is used to re-order reported/commanded joint-position data to accommodate differences between the ROS and robot-controller representations. This capability is also flexible to allow for adding or ignoring "dummy" joint values on either the ROS or robot side.

Usage

The joint-remapping functionality is controlled by providing a list of joint-names to the industrial_robot_client node-base-class init() methods. This list represents the joint names that should be associated with the simple_message protocol communication to the robot controller. The list of names should match the order (and count) of joint data provided/expected by the robot controller's simple_message server code.

If no joint_names list is provided to the init() function, the node will attempt to read a list from the controller_joint_names parameter, as specified here. If this parameter doesn't exist, the node will assume a 6DOF robot, with joint names joint_1, joint_2, ... joint_6.

The provided list of joint names should match the names provided in the robot's URDF file, to be compatible with other ROS navigation/planning nodes. The ordering may be different and individual joints may be added/omitted, depending on the robot-controller's simple_message server requirements.

Examples

Once this joint-name list is provided, the nodes will automatically adjust the published/received data to match. Specific examples are given below:

Standard 6-DOF robot

If a standard 6-DOF robot's URDF uses standard joint names (joint_1, ...) and the robot controller expects the joints in base-to-tip order, no additional setup is required. The nodes will automatically publish 6DOF joint positions under the standard names, and will pass-through the commanded joint positions in joint order 1-6.

Joint Renaming

Some robots (e.g. Motoman) specify their joints using a non-standard naming convention. Motoman joints are specified using letters (S,L,U,R,B,T) rather than numbers (1,2,3,4,5,6). If the URDF file is specified using a non-standard naming convention, then joint-names should be specified to the industrial_robot_client nodes.

The easiest method is to create a robot-specific launch file that sets the controller_joint_names parameter then calls the generic launch file:

<!-- motoman_streaming_interface.launch -->

<rosparam param="controller_joint_names">['joint_s','joint_l','joint_u','joint_r','joint_b','joint_t']</rosparam>
<include file="$(find industrial_robot_client)/launch/robot_interface_streaming.launch" />

Joint Reordering

This capability can also accommodate robot controllers that specify joints in a non-standard order. For example, the motoman controller specifies the SIA20D's 7th axis after the "standard" 6 axes, even though it is physically placed in the middle of the arm. This is easily handled by specifying the joint-name list as in the previous example, using the order expected by the motoman controller:

<rosparam param="controller_joint_names">['joint_s','joint_l','joint_u','joint_r','joint_b','joint_t','joint_e']</rosparam>

Even though the robot URDF specifies the joints in a different order (S,L,E,U,R,B,T), the joint-reordering is handled in the industrial_robot_client nodes using the joint_names list.

Extra Joints

There may be "extra" joints specified by the robot controller that are not defined in the robot's URDF file. These are handled by assigning blank names in the joint_names list.

  • when publishing joint_states, these "dummy" joints are ignored and not published
  • when commanding new joint moves, a default (constant) value is used for the dummy joint
    • NOTE: this behavior may be unsafe, and may require a different default joint position, depending on the robot's configuration.

rosparam set controller_joint_names "['joint_1','joint_2','','joint_3','joint_4','joint_5','joint_6']"

It is not correct to specify some other name (e.g. 'dummy') for the extra joint. The JointTrajectoryInterface will not find the expected 'dummy' joint in the incoming ROS trajectories and will abort the motion. All joints specified in the joint_names list must be included in trajectory commands received from other ROS nodes.

Missing Joints

The robot controller may expect fewer joints than are specified in the URDF file. This would be the case if multiple different robot controllers are used to control a single ROS "robot". In this case, the joint_names list specifies only the subset of joints expected by the connected robot controller

  • when publishing joint_states, all specified joints will be published
    • NOTE: This may cause issues with other ROS nodes, if all expected URDF joints are not present in the published state-stream

  • when commanding new joint positions, only the specified subset of joints will move. Other joint-commands will be ignored.

rosparam set controller_joint_names "['joint_1', 'joint_2', 'joint_3']"

Wiki: industrial_robot_client/joint_naming (last edited 2013-02-13 16:55:00 by GvdHoorn)