Add fcl_mesh collision detector for soft body dynamics