Planning to a joint space goal