adding lfd+trajopt code
merged jointplayback