Defining messages for structure, current state, and commanding arbitrary robots

[ahundt]

@SalvoVirga @marcoesposito1988 @gavanderhoorn @tdinesh @rkojcev @adeguet1 @haudren @gergondet

robot structure, states and commands

Goal:

Define new messages, or determine the correct existing messages that can carry (1) the kinematic graph of an arm (necessary for tool changes) (2) the current state (position, velocity, etc) of that graph (3) command future states of that graph, which are fulfilled by the driver (i.e. the physical arm moves).

Basic Reason:

There are a number of inconsistent messages for driving the kuka iiwa ex1: iiwa_stack, ex2: grl, ex3: majorana.

Stretch Reason:

Could standardized messages be well designed and consistent enough to accommodate additional robot hardware beyond the iiwa?

Difficulties in recording state and commanding robots

• Arbitrary unit definitions
  • ex: proportional force/velocity joint commands from 0-1 as "proportion of robot capability"
• Variation in attachments, tools, etc
• Closed vs open chains (loops/parallel joints)
• trajectories vs real time servoing
  • many robots don't allow real time servoing
• special modes/configuration (not yet addressed here)
  • ex: the kuka iiwa alone has: SmartServo, DirectServo, FRI realtime commands (joint space, position, torque, cartesian), point to point motions, and more.
• latency and variation in guarantees (or lack thereof) supplied by various vendors and APIs
• Specific robots have specific problems to handle
  • even the iiwa precursor the LBR4+ is controlled via completely different APIs
  • da vinci has cables so kinematics change with wear (not yet addressed here)
  • UR5, PR2, other robot gotchas?

Why not just existing ROS messages + URDF files?

Maybe that's possible but right now there is wide variation in what libraries use for similar data, such as these issues discussing this topic for the iiwa:

• Integrate / migrate to ros-industrial/majorana msgs/srvs? IFL-CAMP/iiwa_stack#42
• First set of proposed msgs for setting up Cartesian Impedance parameters #1 and Proposed msgs for Cartesian Impedance #2
• many links at the bottom of this post for other robots

I created this issue here in majorana because it seems to be the place where there is hope of defining arm command messages for ROS. I'm going to need something equivalent (both ros and non-ros), and I've looked at many different classes and message formats for defining robots, transforms, and so on.

Perhaps use graph design of RBDyn SpaceVecAlg and Tasks libraries?

A family of libraries from Joint Japanese-French Robotics Laboratory seems particularly compelling:

• RBDyn.
  • RBDyn allows definition of arbitrary open and closed chains via the combination of MultiBody objects assembled in a MultiBodyGraph
  • the current state of one chain (or perhaps a command) can be specified via a MultiBodyConfig
  • Note the tutorial is slightly out of date as the integer handle accessors have been replaced with much easier to understand string based accessors.
• SpaceVecAlg
  • Defines a spatial vector algebra for rigid bodies
• Tasks
  • Object based objectives and constraints, for constrained optimization model based control approaches

They are based on the Roy Featherstone Rigid Body Dynamics Algorithms book.

SVA/RBDyn Tutorials:

• SVA+RBDyn turorial pdf
• SVA+RBDyn tutorial repo

For other types of transforms and forces the Tasks and SpaceVecAlg sister libraries have interesting definitions. Based on the various class member variables they appear relatively independent and easy to serialize, though quat/trans or other equivalent definitions are possible.

jrl-umi3218/RBDyn#19 includes a relevant discussion of similar serialization via either protobufs or flatbuffers.

If the messages can be defined with easy to use converters, then this may be a way to cleanly deal with the particulars of different robots. Arrays of strings could define each of the sets of joints, bodies, etc. Then similar corresponding elements that can define mappings could be used to serialize the multibody graph data.

The RBDyn data layout definitely appears somewhat complex at first, but in many ways it is also simple and complete. Clear loading unloading, and accessor classes/functions would need to be designed to convert the messages to/from the underlying graph formats to say a simple array if your robot is an open kinematic chain.

Other relevant robot code

Other unreleated reference code for comparison, some for robot state/commands, others not:

• ROS control (ros_control github, std msgs, geometry2 aka tf2
• ros2 std msgs and design
• protobuf based transform handling from googlecartographer (various other datums like pose graphs are in other folders)
• ignition messages
• Hardware Abstraction Library
• iiwa_stack
• grl flatbuffers
• may not have messages, but it have some relevance: openrave, KDL - JHU cisst, dvrk, and dvrk-ros
• UR5 ur_modern_driver

Do you have interest in standardizing, or thoughts on how to do so?

Even the a common approach doesn't turn out to be ideal, I wanted to at least discuss this as a possible source of inspiration, so thanks for taking a look!

[gavanderhoorn]

Thank you for the extensive OP, but could you summarise your goal(s) in one or two lines?

Right now I have a hard time identifying actionable items, which makes it hard for me to prioritise this.

[gavanderhoorn]

@shaun-edwards.

[ahundt]

Goal:

Define new messages, or determine the correct existing messages that can carry (1) the kinematic graph of an arm (2) the current state (position, velocity, etc) of that graph (3) command future states of that graph, which are fulfilled by the driver (i.e. the physical arm moves).

Basic Reason:

There are a number of inconsistent messages for driving the kuka iiwa ex1: iiwa_stack, ex2: grl, ex3: majorana.

Stretch Reason:

Could standardized messages be well designed and consistent enough to accommodate additional robot hardware beyond the iiwa?