Made new OCPlanner work with psi system. Enable feeding psi demanding goals to  OCPlanner.