def seed_cost(x0, u0, Q, R, Q_N, param):

    """ Compute cost 
    Input: trajectories x0 and u0, penalty matrices Q, R, Q_N, parameter structure param
    Output: cost J 
    """
    J = 0
    N = len(u0)
    for k in range(N):
        J = J + (x0[:, k]-param.h_r).T @ Q @ (x0[:, k]-param.h_r)\
         + (u0[:, k]- param.u_r[0]) * R * (u0[:, k]-param.u_r[0])
    
    # Terminal cost term
    J = J + (x0[:, N+1]-param.h_r).T @ Q_N @ (x0[:, N+1]-param.h_r)
    
    return J