Fix bug in ScenePublisher

Fix bug:
- do not use robot.robotConfig in ScenePublisher for composite robot.
- in computeRobotPositionPlanar, conversion from (cosine, sine) to quaternion