Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Issue with ACADOS feature not provided by L4CasADi #64

Open
NiccoBonucci opened this issue Mar 4, 2025 · 0 comments
Open

Issue with ACADOS feature not provided by L4CasADi #64

NiccoBonucci opened this issue Mar 4, 2025 · 0 comments

Comments

@NiccoBonucci
Copy link

NiccoBonucci commented Mar 4, 2025

Good afternoon,
I'm using L4CasADi to convert a pytorch learned dynamical model an use it inside an MPC structure with ACADOS. When using an external cost function, i get this error message after running the script:

terminate called after throwing an instance of 'std::invalid_argument'
what(): jac_adj_i0_adj_o0 is not provided by L4CasADi. If you need this feature, please contact the L4CasADi developer.
Aborted (core dumped)

This is the model structure using casADi:

class DriftDynamicsModel:

def __init__(self, learned_dyn):
    self.learned_dyn = learned_dyn  # Sposta il modello su GPU

    # Convert min/max values to CasADi DM (constant matrices)
    self.in_min = cs.DM(in_min)  
    self.in_max = cs.DM(in_max)

    self.out_min = cs.DM(out_min)  
    self.out_max = cs.DM(out_max)

    self.param_min = cs.DM(param_min)
    self.param_max = cs.DM(param_max)

    # Create symbolic variables
    x_sym = cs.MX.sym('x', len(in_min))
    p_sym = cs.MX.sym('p', len(param_min))
    x_norm_sym = cs.MX.sym('x_norm', len(out_min))

    # Normalization function for states (-1 to 1) with zero-division check
    self.normalize_input = cs.Function(
        'normalize_input',
        [x_sym],
        [cs.if_else(
            (self.in_max - self.in_min) == 0,  # Avoid division by zero
            -1,  # Return 0 if min and max are equal
            2 * ((x_sym - self.in_min) / (self.in_max - self.in_min)) - 1
        )]
    )

    # Normalization function for parameters (-1 to 1) with zero-division check
    self.normalize_params = cs.Function(
        'normalize_params',
        [p_sym],
        [cs.if_else(
            (self.param_max - self.param_min) == 0,  # Avoid division by zero
            -1,  # Return 0 if min and max are equal
            2 * ((p_sym - self.param_min) / (self.param_max - self.param_min)) - 1
        )]
    )

    # Denormalization function for states with zero-division check
    self.denormalize_output = cs.Function(
        'denormalize_output',
        [x_norm_sym],
        [ cs.if_else(
            (self.out_max - self.out_min) == 0,  # Avoid division by zero
            self.out_min,  # Return min if min and max are equal
           (( (x_norm_sym + 1) * (self.out_max - self.out_min)) / 2 ) + self.out_min ) ]
    )

def model(self, x0):
    world_x = cs.MX.sym('world_x', 1)
    world_y = cs.MX.sym('world_y', 1)
    world_heading = cs.MX.sym('world_heading', 1)
    local_vx = cs.MX.sym('local_vx', 1)
    local_vy = cs.MX.sym('local_vy', 1)
    slip_angle = cs.MX.sym('slip_angle', 1)
    yaw_rate = cs.MX.sym('yaw_rate', 1)

    ref_x = cs.MX.sym('ref_x', 1)
    ref_y = cs.MX.sym('ref_y', 1)
    ref_heading = cs.MX.sym('ref_heading', 1)
    
    steer = cs.MX.sym('steer', 1)
    throttle = cs.MX.sym('throttle', 1)
    
    state = cs.vertcat(world_x, world_y, world_heading, local_vx, local_vy, yaw_rate)
    controls = cs.vertcat(steer, throttle)
    
    deltatime = cs.MX.sym('deltatime', 1)
    
    params = cs.vertcat(ref_x, ref_y, ref_heading, slip_angle, deltatime)

    input_state = cs.vertcat(world_x, world_y, world_heading, local_vx, local_vy, slip_angle, yaw_rate)

    input_state_norm = self.normalize_input(input_state)
    inputs_vector = cs.vertcat(input_state_norm, controls, deltatime).T

    outputs_norm = self.learned_dyn(inputs_vector).T
    outputs = self.denormalize_output(outputs_norm)

    next_x = state[0] + outputs[0]
    next_y = state[1] + outputs[1]
    next_heading = state[2] + outputs[2]
    next_local_vx = outputs[3]
    next_local_vy = outputs[4]
    next_yaw_rate = outputs[5]
    
    # **Chiamata alla rete neurale**
    next_state = cs.vertcat(next_x, next_y, next_heading, next_local_vx, next_local_vy, next_yaw_rate)
    
    #print("Shape of next_state:", next_state.shape)
    assert next_state.shape == (6, 1), f"Expected next_state to have shape (, 1), got {next_state.shape}"

    #print("Discrete dynamics expression:", next_state)

    model = cs.types.SimpleNamespace()
    model.x = state
    model.u = controls
    model.p = params
    model.x0 = x0
    model.name = "car_nn_model"
    model.disc_dyn_expr = next_state
    
    return model

This is the function to define the cost function:

def define_external_cost(model, weights):

# State variables
world_x = model.x[0]
world_y = model.x[1]
world_heading = model.x[2]  

# Reference values from parameters
ref_x = model.p[0]  # Reference x
ref_y = model.p[1]  # Reference y
ref_heading = model.p[2]  # Reference heading

# === Compute Quadratic Errors ===

# 1. Quadratic error in x-position
x_cost_element = (world_x - ref_x) ** 2

# 2. Quadratic error in y-position
y_cost_element = (world_y - ref_y) ** 2

# 3. Quadratic error in heading, using angular difference
error_heading = world_heading - ref_heading
error_heading = cs.fmod((error_heading) + np.pi, 2 * np.pi) - np.pi
heading_cost_element = (error_heading) ** 2

# === Total Cost Function ===
cost_expr = weights[0]*x_cost_element + weights[1]*y_cost_element + weights[2]*heading_cost_element

return cost_expr

What could the problem be? Is this feature not present in the current version?

@NiccoBonucci NiccoBonucci changed the title Issue with ACADOS parameter not provided by L4CasADi Issue with ACADOS feature not provided by L4CasADi Mar 4, 2025
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant