removed Infinite loop. added pi/2 for starting pose.