TRL5 prototype of motion planner for ropod

Summary
TRL5 prototype and documentation of motion planner for ropod moving in time-varying environment