TrajPlan aim to compute a collision free trajectory between two robot posture