# Dynamics Formulation for Manipulator MPC

Hey there! First of all thank you so much for your great work and effort you put into it!

I am currently trying to implement a MPC-Approach for a robotic manipulator. Since my control values must be joint torques, my system dynamics equal to the forward dynamics problem in which I solve for joint acceleration qddot. My system states however are x = [q, qdot], so my derived states equal to xdot = [qdot, qddot], which in turn corresponds to the FD. My question now lies in how I shall define the dynamics (f_expl) for the acados solver?

I am able to export a symbolic expression for qddot with urdf2casadi. But how do i define the integration step for qdot? my first guess would be to init the state vector with

``````states = cs.vertcat(q_sym, q_dot_sym) #x

states_dot = cs.vertcat(q_dot_sym, q_ddot_sym) #xdot
``````

``````q_ddot_expr = robot.get_forward_dynamics_aba()
``````

and finally define system dynamics with f_expl as

``````q_dot_func = q_dot_sym #just use integrated state as function for q_dot
f_expl = cs.vertcat(q_dot_sym_func, q_ddot_expr )
``````

Would this be the way you would proceed to do it? Or is there another way to include a state dependency like the one in my example?

Best regards
Thorben

Hi Thorben,

in principle this should work!
You should be careful that the `states` and `states_dot` don’t contain the same symbolic variables.

So, this is not expected to work:

``````states = cs.vertcat(q_sym, q_dot_sym) #x
states_dot = cs.vertcat(q_dot_sym, q_ddot_sym) #xdot
``````

since `q_dot_sym` is contained in both.

The same logic is here for the state `theta`: