Current State Monitor and Multiple DOF Joints

[Jmeyer1292]

The current moveit state monitor appears to ignore multi-dof joints that are not at the root of the RobotModel. Please see here.

We in the ROS-Industrial world would like to use floating joints to enable the dynamic calibration of robots and targets to their environments. While this system can be faked by creating 6 single-dof joints that approximate a floating joint and publishing values for all of them, this is far from ideal.

If my understanding of the situation is correct, I propose to create an extension to the existing current_state_monitor that:

1. Creates a new callback that services multi-DOF joints. This could be tied to the existing joint_state callback or could be powered independently by a timer, subscribing to /tf, or through a tf listener.
2. This callback would use RobotModel's getMultiDOFJointModels() function to obtain a list of joints to update, then it would perform TF lookups for each of them, and finally update MoveIt's state in the same manner as is done currently here.

I'm very interested in your opinions. I would be willing to do the work, but I'd like to agree on a course of action. I do NOT imagine that these joints would be actively planned for as part of this change, merely that they would be updated between planning requests and otherwise treated as fixed joints.

[davetcoleman]

Very good points about the current limitations of the CurrentStateMonitor! My main questions are:

• Will there be causes when a multi-dof joint is not updated by TF but instead by something like a sensor_msgs/JointState message? I realize that msg currently only supports single joints.
• Do we need a callback or should we just implement this for our current use case in away that is useful to everyone? Callbacks seem messy
• Just out of curiosity, could you give an example of what calibrated component would need to be updated between planning requests? Do you mean like a workbench to robot calibration?

Thanks for volunteering to add this feature!

[Jmeyer1292]

Hey Dave,

Thanks for taking the time to respond. I would like to be able to do a precise calibration of a tool or camera at runtime, update its transformation, and have the collision models update accordingly. It is also possible that part of the world geometry changes position or orientation and the collision models need to be precisely updated based on some metrology test. In these scenarios, the existence of geometry is known ahead of time and can be succinctly expressed in a URDF with a floating joint. The exact pose of said geometry can change during runtime.

A recent PR to robot_state_publisher causes that node to explicitly ignore floating joints. joint_state_publisher has similar logic. So while it is possible that you could update the multi-DOF joints via a sensor_msgs/JointState, I don't see any infrastructure to help make that happen.

Would you please provide more details by what you mean by your second bullet point? I proposed a separate callback to go with the existing JointStates one so that the TF powered joints could be updated at a different rate than the JointState powered ones.

Thanks.

[davetcoleman]

I don't see any infrastructure to help make that happen.

Great point

please provide more details by what you mean by your second bullet point?

I didn't look at the current implementation, but if its already using a callback then nevermind, it makes sense to add another one. Thanks!