New publishRobotGraph function
New convertRobotStatesToTipPoints function