Added TimeStateSpace to SE2MultiBodyRigidPlaning to create SE2TimeMultiRigidBodyPlanning