Changes to the RigidBodyPlanning.cpp to act as a testbed for the new planner.
Added the equivalent code for the worker_main and create_worker_system.