Solving a boundary value problem via multiple shooting

User Questions
1

Hello,

I am using the Python interface to solve a boundary value problem via multiple shooting method.

The ordinary differential equations implemented are discretized w.r.t. an arc length instead of time.

I wrote a code solver to determine the best subset of initial conditions such that the boundary conditions can be minimised.

However, solving the NLP will always result in the QP solver return error status 3.

I have tried the following:

  1. Setting an approximately close solution for an initial guess for all stages.
  2. Setting a regularization term.
  3. Checking the cost function.

If it helps, the multiple shooting problem is formulated into an OCP solver as such:

        self.ocp = AcadosOcp()
        self.ocp.model = self.tetherObject.model
        nx = self.tetherObject.model.x.size()[0]
        nu = self.tetherObject.model.u.size()[0]
        ny = nx + nu

        self.ocp.dims.N = self.integration_steps
        # self.ocp.cost.cost_type_0 = 'LINEAR_LS'
        # self.ocp.cost.cost_type = 'LINEAR_LS'
        self.ocp.cost.cost_type_e = 'LINEAR_LS'
        self.ocp.cost.W_e = np.identity(nx)
        self.ocp.cost.W = np.zeros((ny, ny))
        self.ocp.cost.Vx = np.zeros((ny, nx))
        self.ocp.cost.Vx_e = np.zeros((nx, nx))
        self.ocp.cost.Vx_e[0:7, 0:7] = np.identity(7)
        self.ocp.cost.yref  = np.zeros((ny, ))
        self.ocp.cost.yref_e = np.zeros((nx))
        self.ocp.cost.Vu = np.zeros((ny, nu))
        self.ocp.solver_options.qp_solver_iter_max = 400
        # self.ocp.solver_options.sim_method_num_steps = self.integration_steps
        self.ocp.solver_options.qp_solver_warm_start = 2
        self.ocp.solver_options.levenberg_marquardt = 1.0
        self.ocp.solver_options.regularize_method = 'MIRROR'

        self.ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # 
        # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM,
        # PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP, FULL_CONDENSING_DAQP
        self.ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' 
        self.ocp.solver_options.integrator_type = 'ERK'
        # self.ocp.solver_options.print_level = 1
        self.ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP_RTI, SQP
        self.ocp.solver_options.tf = self.boundary_length

        wrench_lb = -2
        wrench_ub = 2

        self.ocp.constraints.idxbx_0 = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16])

        self.ocp.constraints.lbx_0 = np.array([0, 0, 0, 1, 0, 0, 0, #pose at start.
            wrench_lb , wrench_lb , wrench_lb , wrench_lb , wrench_lb , wrench_lb , # internal forces at start.
            0, 0, 0.05, 0]) #tau, alpha, kappa, curvature

        self.ocp.constraints.ubx_0 = np.array([0, 0, 0, 1, 0, 0, 0, #pose at start.
            wrench_ub , wrench_ub , wrench_ub , wrench_ub , wrench_ub , wrench_ub , # internal forces at start.
            0, 0, 0.05, 0]) #tau, alpha, kappa, curvature

        self.ocp.constraints.idxbx = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16])

        self.ocp.constraints.lbx = np.array([-5, -5, -5, -1.05, -1.05, -1.05, -1.05, #pose at start.
            wrench_lb , wrench_lb , wrench_lb , wrench_lb , wrench_lb , wrench_lb , # internal forces at start.
            -20, 0, 0.05, 0]) #tau, alpha, kappa, curvature

        self.ocp.constraints.ubx = np.array([5, 5, 5, 1.05, 1.05, 1.05, 1.05, #pose at start.
            wrench_ub , wrench_ub , wrench_ub , wrench_ub , wrench_ub , wrench_ub , # internal forces at start.
            20, self.boundary_length, 0.05, 50]) #tau, alpha, kappa, curvature

        self.ocp.solver_options.nlp_solver_max_iter = 200

        solver = AcadosOcpSolver(self.ocp, json_file=f'{self.ocp.model.name}.json')
        integrator = AcadosSimSolver(self.ocp, json_file=f'{self.ocp.model.name}.json')

        return solver, integrator

Printing the residuals and statistics from the solver (when putting an approximate solution) gives:

[1.00054985e+00 0.00000000e+00 3.12768808e-01 9.26639719e-09]
iter    qp_stat qp_iter
0       0       0
1       0       4

Without using an approximate solution, I get:

[2.2000000e+00 1.1996702e+35 5.0000000e+01 0.0000000e+00]

iter    qp_stat qp_iter
0       0       0
1       3       1

Thanks for the help.

Jer Luen

1 Like
2

Hi,

for prototyping I would rather use SQP instead of SQP_RTI.
Then you also get the residuals in the statistics.
I am not sure if in the second case the residual is before or after the solution.
But an equality residual of 1.1996702e+35 is really large.
SQP methods often need some decent initialization and something like this is not expected to be in the region of contraction.

Best,
Jonathan

1 Like
3

Hi Jonathan,

I managed to solve the issue by tweaking some of the variables in the ODE. I have one more question. Does acados support the retrieval of forward sensitivity matrices at every integration step? Or are there any methods that allow the user to obtain the solution of an ODE at every time step using the integrator interface?

Thank you for the massive help.

Jer Luen

4

Hi,

great to hear that you solved it.

I added some functionality to retrieve the QP matrices (including the linearized dynamics) here:

Getting intermediate results from the integrators is not supported for now.

Cheers!