Working on building a software layer for manipulators, finally got it working for simulation and physical!