Use fcl instead of the homogeneous matrices from sot-core