Implementing reverse dynamics-not done yet