Updating system dynamics for each stage in python

MPC Discussions
1

Hi,

I’m using python to solve NONLINEAR_LS cost functions for a continuous quadrotor system. My original setup works well, but now I’m going to add a time varying offset for further testing.

I plan add an offset for self.u (i.e., f_thrust) only to some stages (e.g., the first stage) of the model below, so I wonder how I can do this. I found a post Change model parameters - User Questions - acados forum that mentioned <model_name>_acados_update_params, but I cannot find the detailed usage anywhere.

Is there any instructions to update system dynamics like this? Thanks!

def quad_dynamics(self):
    x_dot = cs.vertcat(self.x_dynamics(), self.v_dynamics(), self.q_dynamics(), self.w_dynamics())
    return cs.Function('x_dot', [self.x[:13], self.u], [x_dot], ['x', 'u'], ['x_dot'])

def x_dynamics(self):
    return self.v

def q_dynamics(self):
    return 1 / 2 * cs.mtimes(skew_symmetric(self.w), self.q)

def v_dynamics(self):
    f_thrust = self.u
    g = cs.vertcat(0.0, 0.0, self.g)
    ind = True
    if ind:
        a_thrust = cs.vertcat(0.0, 0.0, f_thrust[0]+f_thrust[1]+f_thrust[2]+f_thrust[3]) / self.quad_mass
    else:
        a_thrust = cs.vertcat(0.0, 0.0, f_thrust[0]) / self.quad_mass
    v_dynamics = v_dot_q(a_thrust, self.q) - g
    return v_dynamics

def w_dynamics(self):
    f_thrust = self.u
    ind = True
    if ind:
        TM = f_to_TM(f_thrust, True)
        tau_b = cs.vertcat(TM[1], TM[2], TM[3])
    else:
        tau_b = cs.vertcat(f_thrust[1], f_thrust[2], f_thrust[3])
    tmp = tau_b - cs.cross(self.w, cs.mtimes(self.J, self.w))
    return cs.mtimes(self.inv_J, tmp)
2

Hello,

I believe that you could find what you’re looking for in the chain mass example. As Jonathan mentioned here, you will need to define a parameter (e.g., u_offset) and set it to the desired value before solving the OCP.

An example of adding a parameter to the dynamics can be found here and changing it online can be found here.

To set it only for the first stage, you should add the stage number as shown here, i.e. <your_solver>.set(0, "p", <desired_value>).

Hope this helps!

1 Like
3

Thanks, I’ll have a try!