Modifed trajectory planner to use a ModelRecManager and made work with staubli arm
added ModelRecManager files
Fixing merge