Fix bug regarding robotConfig in scene_publisher
Fix bug regarding planar joint.