Fix initial configuration

  Previous initial configuration was not in the joint bounds.