Exemple #1
0
    def solve_at_priority_regular(self, priority, fix_sampled_params=False, recently_sampled=None):
        # initialize gurobi Model object
        prob = OptProb()
        self.recently_converged_vio_fluent = None
        if recently_sampled is None:
            recently_sampled = []

        params = set()
        for hla in self.hlas:
            params.update(hla.get_params())
            prob.add_hla(hla)

        # check whether there are duplicate parameters (prob shouldn't add variables into the problem multiple times)
        param_to_var = {}
        for param in params:
            if param in param_to_var:
                continue
            if param.is_resampled and fix_sampled_params:
                print param.name, "is fixed."
                const = Constant(param)
                param_to_var[param] = const
            elif not param.is_var:
                print param.name, "is not a var."
                const = Constant(param)
                param_to_var[param] = const
            else: # param.is_var
                var = Variable(param, recently_sampled=(param in recently_sampled))
                param_to_var[param] = var
                prob.add_var(var)
        prob.update()

        # max(priority, 0) because priority = -1 used for straight-line init
        self.add_cnts_to_opt_prob(prob, param_to_var=param_to_var, priority=max(priority, 0))
        
        # Re-sampled params need to update their corresponding variables
        for param, var in param_to_var.items():
            var.set_val(param.get_value())

        for hla in self.hlas:
            if hla.cost != 0.0:
                hla.cost.to_gurobi_fn(prob, param_to_var)
                prob.inc_obj(hla.cost)

        solver = Solver()

        if priority == -1 or priority == 0:
            # for initialization only because problem should be QP
            solver.initial_trust_box_size = 1e5
            solver.max_merit_coeff_increases = 1
        if priority == -1:
            # initialize straight-line trajectories
            success = prob.initialize_traj(mode="straight")
        elif priority == 0:
            # initialize from adapted previous trajectories
            success = prob.initialize_traj(mode=settings.INIT_MODE)
        else:
            if settings.DO_SQP or settings.BACKTRACKING_REFINEMENT or settings.BTSMOOTH_REFINEMENT:
                do_early_converge = False
            elif settings.DO_EARLY_CONVERGE:
                do_early_converge = True
            else:
                raise NotImplementedError
            success, converged_vio_fluent = solver.penalty_sqp(prob, do_early_converge=do_early_converge)
            if not do_early_converge:
                assert converged_vio_fluent is None
            self.recently_converged_vio_fluent = converged_vio_fluent

        for param, var in param_to_var.items():
            var.update_hl_param()

        self.traj_cost = sum(prob.val(0)[0])
        return success
Exemple #2
0
    def solve_at_priority_admm(self, priority, fix_sampled_params=False, recently_sampled=None):
        # initialize gurobi Model object
        if recently_sampled is None:
            recently_sampled = []

        hla_to_prob = {}
        param_to_var = {}
        # construct opt probs for each action and create variables for each parameter
        for hla in self.hlas:
            prob = OptProb()
            prob.add_hla(hla)
            hla_to_prob[hla] = prob

            params = hla.get_params()
            for param in params:
                if param.is_resampled and fix_sampled_params:
                    if param not in param_to_var:
                        # print param.name, "is fixed."
                        const = Constant(param)
                        param_to_var[param] = const
                elif not param.is_var:
                    if param not in param_to_var:
                        # print param.name, "is not a var."
                        const = Constant(param)
                        param_to_var[param] = const
                else: # param.is_var
                    if param in param_to_var:
                        var = param_to_var[param]
                    else:
                        if param.is_resampled:
                            # print param.name, "is a global var."
                            var = GlobalVariable(param, recently_sampled=(param in recently_sampled))
                            param_to_var[param] = var
                        else:
                            # print param.name, "is a var."
                            var = Variable(param, recently_sampled=(param in recently_sampled))
                        param_to_var[param] = var

                    prob.add_var(var)
            prob.update()
            self.add_cnts_to_opt_prob(prob, param_to_var=param_to_var, priority=max(priority, 0))

        for hla, prob in hla_to_prob.items():
            if hla.cost != 0.0:
                hla.cost.to_gurobi_fn(prob, param_to_var)
                prob.inc_obj(hla.cost)

        solver = Solver()

        if priority == -1 or priority == 0:
            # for initialization only because problem should be QP
            solver.initial_trust_box_size = 1e5
            solver.max_merit_coeff_increases = 1
        if priority == -1:
            # initialize straight-line trajectories
            for prob in hla_to_prob.values():
                success = prob.initialize_traj(mode="straight")
        elif priority == 0:
            # initialize from adapted previous trajectories
            for prob in hla_to_prob.values():
                success = prob.initialize_traj(mode="adapt")
        else:
            success = solver.sqp_qp_admm(hla_to_prob.values(), param_to_var.values())

        for param, var in param_to_var.items():
            var.update_hl_param()

        self.traj_cost = 0
        for prob in hla_to_prob.values():
            self.traj_cost += sum(prob.val(0)[0])
        return success