Rigid body pose and twist scene graph founded on geometric relations semantics for robotic applications

This paper presents a scene graph for geometric relations between rigid bodies that keeps track of poses and twists of rigid bodies in the scene. The scene graph relies on semantic pose and twist representation, making it invariant to the actual coordinate representation at hand. This makes the scene graph more general and interoperable than most scene graphs currently available. The presented scene graph takes into account constraints imposed by particular coordinate representations, allows for constant poses, answers semantic pose and twist queries, and provides built-in semantic consistency checks. Since the scene graph also keeps track of the twist, it allows native twist calculations, as opposed to deriving the velocities from the poses in the graph. This paper comes with software released under a dual BSD/LGPLv2.1 license.