forward and inverse dynamics update