progress on making the robot dynamics work (still buggy)