Motion planning and geometric reasonning for human-robot interaction based on libmove3d