    def obtain_solution(self, cur_state, init_mean=None, init_var=None):
        """ Optimizes the cost function using the provided initial candidate distribution
        :param cur_state: Full state of the current environment such that the environment can always be reset to this state
        :param init_mean: (np.ndarray) The mean of the initial candidate distribution.
        :param init_var: (np.ndarray) The variance of the initial candidate distribution.
        :return:
        """
        mean = (self.ub + self.lb) / 2. if init_mean is None else init_mean
        var = (self.ub - self.lb) / 4. if init_var is None else init_var
        t = 0
        X = stats.norm(loc=np.zeros_like(mean), scale=np.ones_like(mean))

        while (t < self.max_iters):  # and np.max(var) > self.epsilon:
            print("inside CEM, iteration {}".format(t))
            samples = X.rvs(size=[self.population_size, self.solution_dim]) * np.sqrt(var) + mean
            samples = np.clip(samples, self.lb, self.ub)
            full_samples = np.tile(samples, [1, self.plan_n_segs])
            costs_ = self.cost_function(cur_state, full_samples)
            costs = [_[0] for _ in costs_]
            print(np.mean(costs), np.min(costs))
            sort_costs = np.argsort(costs)

            elites = samples[sort_costs][:self.num_elites]
            mean = np.mean(elites, axis=0)
            var *= 0.2
            t += 1
        sol, solvar = mean, var
        sol = np.tile(sol, self.plan_n_segs)
        solvar = np.tile(solvar, [1, self.plan_n_segs])
        return sol
Optimizes the cost function using the provided initial candidate distribution
:param cur_state: Full state of the current environment such that the environment can always be reset to this state
:param init_mean: (np.ndarray) The mean of the initial candidate distribution.
:param init_var: (np.ndarray) The variance of the initial candidate distribution.
:return:
execute_locomotion.py