This PR refact the action and cost classes in order to used reference instead of pointers. Additionally, for the cost classes, the API has been changed. Now we need to pass the state multibody. Finally, I cleaned up the extra piece of costs, e.g. getter function of nq in action or cost classes.