Dynamics Formulation for Manipulator MPC

User Questions
1

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
Thorben

2

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:

3

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