Added elements for including interpolated states and their associated movements - unfinished (mainly requires obstacle and movement constraint checks)