Updates to BITStar double integrator (goal region, g_T, cached calls). Tried FORCES Pro