Got the system working (except for the IK)