Reducing Computational Expensiveness of symbolic expressions for a system of dynamic equations when solving MPC

[shulnak09]

Hi all,

I am trying to solve a MPC using second order dynamics model for a manipulator. The manipulator has 7 DOF and I used a tool box to derive the dynamics equation analytically which in includes the mass (M), coriollis (C) and gravity terms (G). I have ignored the friction terms as it is hard to get a very accurate model considering all the parameters. For the M (7 x 7) ,C (7 x 7 ) and G (7 x 1) matrices, I get 1000 of lines of symbolic expressions in terms of joint angles as below:

# Joint angles 
q_0 = ca.SX.sym('q_0')
q_1 = ca.SX.sym('q_1')
q_2 = ca.SX.sym('q_2')
q_3 = ca.SX.sym('q_3')
q_4 = ca.SX.sym('q_4')
q_5 = ca.SX.sym('q_5')
q_6 = ca.SX.sym('q_6')
q =  ca.vertcat(q_0, q_1, q_2, q_3, q_4, q_5, q_6)

M = -0.045792*sin(q_1)*sin(q_2)*sin(q_3)*sin(q_4)*sin(q_5)*cos(q_1)*cos(q_4)*cos(q_5)*cos(q_6) - 1.38777878078145e-17*sin(q_1)*sin(q_2)*sin(q_3)*sin(q_4)*cos(q_1)*cos(q_3)*cos(q_4)*cos(q_5)**2 + 0.03498*sin(q_1)*sin(q_2)*sin(q_3)*sin(q_4)*cos(q_1)*cos(q_4)*cos(q_5)**2*cos(q_6) ....  # 1000 lines of symbolic expression

I have few questions regarding this:
a. I am creating casadi functions in a separate .py file using the expression as below and calling the f_xu into my main MPC loop where it is called to formulate the objective function. It takes a lot of time to compute the symbolic expressions if they are very large. Is there any suggestion on how can I make this process faster? I can ignore the negligible terms.

qdd = ca.mtimes(M_inv, (control - C@qd -G))
rhs = ca.vertcat(qd_0, qd_1, qd_2, qd_3, qd_4, qd_5, qd_6, qdd)

f_xu = ca.Function('f_xu', [g,q,qd], [rhs])

b. Is there any other alternative ways in casadi that I can leverage to solve the MPC without using analytical equations for the dynamics model. I have the numerical M, C, G values for a particular q and q_dot.

[sandeep026]

Can you check the number of nodes in the expression graph? If high split up as functions and checkpoint.

[shulnak09]

Hi Sandeep,

I was checking the online groups to see ways to obtain the nodes in expression graph, i printed the solver and got the result as below. Aparently, the solver takes 7s to get formulated. Let me know if this is what you asked.

solver:(x0[192],p[24],lbx[192],ubx[192],lbg[132],ubg[132],lam_x0[192],lam_g0[132])->(x[192],f,g[132],lam_x[192],lam_g[132],lam_p[24]) IpoptInterface

But, I have some larger issues which I have highlighted in the code below and my queries, any help would be really helpful.

1. Since, I use an external library like sympy to derive the dynamical equations of a large robotic system and then convert those sym equations to casadi.SX, the overall computation becomes time consuming. Is there any alternate ways to formulate dynamical systems in casadi?

2. Also, I have attached the main loop of my code below. Any suggestions on how to improve computation will be really helpful.

## MPC code below
# Define the parameters:
state_init = ca.DM(q0)
state_target = ca.DM(qs)


n_states = 12
n_controls = 6

T = 0.1
N = 10
sim_time = 5
a_g = -9.8  # Gravity

# Define the Weighing factors:
Q = 1 * np.diag([1, 1, 1 , 1, 1, 1 ])
Q_N = np.diag([10, 10, 10, 20, 20, 20 ])
R =  1e-6 * np.diag([0.01, 0.01, 0.01, 1, 1, 1])
S = 1e1  *np.diag([1,1,1,1,1,1]) * 1
alpha_1 = 1
alpha_2 = 1
factor = 10

# Define the full state:
X = ca.SX.sym('X', n_states, N+1)

# Define the controls:
U = ca.SX.sym('U', n_controls, N)

# Define the initial and target states:
P = ca.SX.sym('P', 2*n_states)

st = X[:,0]
obj = 0
g = []
g = ca.vertcat(g, st - P[:n_states])
# End state Pose of the Robot Configuration : fkine_sym is a Symbolic Expression from Python converted to SX
wTep = fkine_sym(P[n_states:n_states+n]) 

for k in range(N):
    st = X[:,k]
    con = U[:,k]
    # grav_torque = Grav_load(a_g, st)

    if k < N-1:
        con_next = U[:,k+1]
        
    # Task Space  Conversion:
    wTe  = fkine_sym(st[:6])  # Sympy expression (fkine_sym) converted to SX later.

    # alpha_1 is multiplied with orientation cost while alpha_2 is multiplied with translation cost. 
    obj += alpha_1 * ca.norm_fro(ca.SX.eye(3) - ca.mtimes(wTep[:3,:3].T, wTe[:3,:3])) \
            + alpha_2 *  (wTe[:3,-1] -  wTep[:3,-1]).T @ Q[3:, 3:] @ (wTe[:3,-1] -  wTep[:3,-1])\
            + (con).T @ R @ (con ) * 1 + (con_next - con).T @ S @ (con_next-con)
    
    # Next Predicted state:
    st_next = X[:, k+1]

    # Integration of dynamical system:
    # USe Trapezoidal Integration scheme:
    '''
    f(x,u) : Dynamics of the robot model. It is huge as I derrive the expressions in
    sympy and convert it to Casadi SX. The overhead at each loop is pretty high.
    a_g: acceleration due to gravity,
    st : state
    con: control
    '''
    k1 = f(a_g, st, con)
    k2 = f(a_g, st + T*k1, con)

    st_next_RK4 = st + T/2 * (k1 + k2)
    st_next_RK4 = st_next_RK4

    g = ca.vertcat(g, st_next -  st_next_RK4) # Equality Constraint


obj += factor * alpha_1 * ca.norm_fro(ca.SX.eye(3) - ca.mtimes(wTep[:3,:3].T, fkine_sym(X[:6, N])[:3,:3])) \
    + alpha_2 *  (fkine_sym(X[:6, N])[:3,-1] - wTep[:3,-1] ).T @ Q_N[3:,3:] @ (fkine_sym(X[:6, N])[:3,-1] - wTep[:3,-1])


OPT_variables = ca.vertcat(
    X.reshape((-1, 1)),
    U.reshape((-1, 1))
)

nlp_prob = {
    'f': obj,
    'x': OPT_variables,
    'g': g,
    'p': P
}

opts = {
    'ipopt': {
        'max_iter': 100,
        'print_level': 0,
        'acceptable_tol': 1e-8,
        'acceptable_obj_change_tol': 1e-6
    },
    'print_time': 0
}

solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts)

Regards,
Anshul

[sandeep026]

ca.n_nodes(obj)

[shulnak09]

Thanks Sandeep.

I just checked the n_nodes with the command above, it comes out to be 72676.

Regards
Anshul

[sandeep026]

High. Split into multiple functions (SX)

[shulnak09]

Thanks. So you are suggesting if I have a large symbolic equation like below which is dependent upon joint angles say q_1, ... q_5, in that case it is better to split up the large equation and create multiple casadi SX equations and then sum them up at the end to create the final equation. Is that correct?

M = [-0.10947*g*sin(q_1)**2*sin(q_3)**2*sin(q_4)*sin(q_6)**2*cos(q_4)*cos(q_5)**2 - 0.10947*g*sin(q_1)**2*sin(q_3)**2*sin(q_4)*cos(q_4)*cos(q_5)**2*cos(q_6)**2 + 0.10947*g*sin(q_1)**2*sin(q_3)**2*sin(q_4)*cos(q_4)*cos(q_5)**2 - 0.10947*g*sin(q_1)**2*sin(q_3)*sin(q_4)*sin(q_5)*sin(q_6)**2*cos(q_3)*cos(q_5) - 0.10947*g*sin(q_1)**2*sin(q_3)*sin(q_4)*sin(q_5)*cos(q_3)*cos(q_5)*cos(q_6)**2 + 0.10947*g*sin(q_1)**2*sin(q_3)*sin(q_4)*sin(q_5)*cos(q_3)*cos(q_5) - 0.1271*g*sin(q_1)*sin(q_2)*sin(q_3)**2*sin(q_5)*sin(q_6)**2*cos(q_1)*cos(q_4)*cos(q_5) - 0.1271*g*sin(q_1)*sin(q_2)*sin(q_3)**2*sin(q_5)*cos(q_1)*cos(q_4)*cos(q_5)*cos(q_6)**2 + 0.1271*g*sin(q_1)*sin(q_2)*sin(q_3)**2*sin(q_5)*cos(q_1)*cos(q_4)*cos(q_5) - 0.1271*g*sin(q_1)*sin(q_2)*sin(q_3)*sin(q_4)**2*sin(q_6)**2*cos(q_1)*cos(q_3) - 0.10947*g*sin(q_1)*sin(q_2)*sin(q_3)*sin(q_4)**2*sin(q_6)**2*cos(q_1)*cos(q_5)**2 - 0.1271*g*sin(q_1)*sin(q_2)*sin(q_3)*sin(q_4)**2*cos(q_1)*cos(q_3)*cos(q_6)**2 - 0.4743*g*sin(q_1)*sin(q_2)*sin(q_3)*sin(q_4)**2*cos(q_1)*cos(q_3) - 0.10947*g*sin(q_1)*sin(q_2)*sin(q_3)*sin(q_4)**2*cos(q_1)*cos(q_5)**2*cos(q_6)**2 +

Regards
Anshul