Move some code from Armar6Skills to RobotAPI / ArmarXObjects