Updated setRobotState() so it sets the MultiDOFJointState like it should.