Adding feature "save p3d wayPoints in a file"
Adding feature "fullbody in manipulation planning"
Adding support for 2 robots setups
Adding TERRT planner