Joint trajectory controller initially working on hardware