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

then get q_ddot_fd_expr from urdf2casadi

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?

Thank you for your help in advance!
Best regards

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:

Maybe this helps, good luck with your acados implementation! :rocket:

Hi there! It indeed helped and solved my issues. thank you! :slight_smile: