diff --git a/bolero/behavior_search/__init__.py b/bolero/behavior_search/__init__.py index 8cd218ec..b16f4aa0 100644 --- a/bolero/behavior_search/__init__.py +++ b/bolero/behavior_search/__init__.py @@ -1,8 +1,9 @@ from .behavior_search import BehaviorSearch, ContextualBehaviorSearch from .black_box_search import (BlackBoxSearch, ContextualBlackBoxSearch, JustOptimizer, JustContextualOptimizer) +from .power import PoWERWithDMP from .monte_carlo_rl import MonteCarloRL __all__ = ["BehaviorSearch", "ContextualBehaviorSearch", "BlackBoxSearch", "ContextualBlackBoxSearch", "JustOptimizer", - "JustContextualOptimizer", "MonteCarloRL"] + "JustContextualOptimizer", "PoWERWithDMP", "MonteCarloRL"] diff --git a/bolero/behavior_search/power.py b/bolero/behavior_search/power.py new file mode 100644 index 00000000..7da668bd --- /dev/null +++ b/bolero/behavior_search/power.py @@ -0,0 +1,236 @@ +# Author: Jan Hendrik Metzen +# Alexander Fabisch + +import numpy as np +import heapq +import dmp +from .behavior_search import BehaviorSearch, PickableMixin +from ..utils.validation import check_random_state, check_feedback +from ..utils.log import get_logger + + +class PoWERWithDMP(PickableMixin, BehaviorSearch): + """Policy learning by Weighting Explorations with the Returns (PoWER). + + This version of PoWER uses a DMP as policy. + + Paper available from + `NeurIPS `_. + Based on the Matlab code of Kober et al.: `source + `_. + + Parameters + ---------- + initial_params : array-like, shape = (n_params,), optional (default: 0s) + Initial parameter vector. + + variance : float, optional (default: 1.0) + Initial exploration variance. + + covariance : array-like, optional (default: None) + A diagonal (with shape (n_params,)) covariance matrix. + + n_samples_per_update : integer, optional (default: 10) + Number of roll-outs that are required for a parameter update. + + reward_transformation : callable, optional (default: identity) + A function that transforms the rewards (usually to the interval [0, 1], + where 1 is best best possible value). PoWER requires the reward + function be an improper probability distribution, i.e. all rewards must + be positive. It can also be a proper probability distribution, i.e. sum + up to one (during an episode?), which will be beneficial for the + learning speed. An example for a reward transformation function is + lambda r: numpy.exp(s*r), where s is a scaling factor and r is the + reward. + + log_to_file: boolean or string, optional (default: False) + Log results to given file, it will be located in the $BL_LOG_PATH + + log_to_stdout: boolean, optional (default: False) + Log to standard output + + random_state : int or RandomState, optional (default: None) + Seed for the random number generator or RandomState object. + """ + def __init__(self, dmp_behavior, variance=1.0, covariance=None, + n_samples_per_update=10, reward_transformation=lambda r: r, + log_to_file=False, log_to_stdout=False, random_state=None): + self.dmp_behavior = dmp_behavior + self.variance = variance + self.covariance = covariance + self.n_samples_per_update = n_samples_per_update + self.reward_transformation = reward_transformation + self.log_to_file = log_to_file + self.log_to_stdout = log_to_stdout + self.random_state = random_state + + def init(self, n_inputs, n_outputs): + """Initialize the behavior search. + + Parameters + ---------- + n_inputs : int + number of inputs of the behavior + + n_outputs : int + number of outputs of the behavior + """ + self.logger = get_logger(self, self.log_to_file, self.log_to_stdout) + self.random_state = check_random_state(self.random_state) + + self.dmp_behavior.init(n_inputs, n_outputs) + self.mean = self.dmp_behavior.get_params() + self.last_mean = self.mean.copy() + self.n_params = len(self.mean) + + # Entries of best_rollouts have the form: + # (return, random-value, parameters, Q values, exploration variance) + self.best_rollouts = [] + + time = np.arange( + 0.0, self.dmp_behavior.execution_time + self.dmp_behavior.dt, + self.dmp_behavior.dt) + phases = [dmp.phase(t, self.dmp_behavior.alpha_z, + self.dmp_behavior.execution_time, 0.0) + for t in time] + self.basis = np.array([self._dmp_activations(z) for z in phases]) + """ + import matplotlib.pyplot as plt + plt.figure() + plt.plot(time, self.basis) + plt.show() + #""" + self.tmp_outer = np.array([ + np.outer(self.basis[i], self.basis[i]) + for i in range(self.basis.shape[0])]) + + if self.covariance is None: + self.cov = np.ones(self.n_params) + else: + self.cov = np.asarray(self.covariance).copy() + self.cov *= self.variance + self.initial_cov = self.cov.copy() + + self.it = 0 + + # TODO this is copied from the DMP implementation... + def _dmp_activations(self, z): + activations = np.exp( + -self.dmp_behavior.widths * (z - self.dmp_behavior.centers) ** 2) + activations /= activations.sum() + return activations + + def get_next_behavior(self): + """Obtain next behavior for evaluation. + + Returns + ------- + behavior : Behavior + mapping from input to output + """ + noise = np.sqrt(self.cov) * self.random_state.randn(self.n_params) + self.params = self.mean + noise + self.dmp_behavior.set_params(self.params) + self.dmp_behavior.reset() + return self.dmp_behavior + + def set_evaluation_feedback(self, feedbacks): + """Set feedback for the last behavior. + + Parameters + ---------- + feedbacks : list of float + feedback for each step or for the episode, depends on the problem + """ + rewards = check_feedback(feedbacks) + rewards = self.reward_transformation(rewards) + q = rewards[::-1].cumsum()[::-1] + """ + import matplotlib.pyplot as plt + plt.plot(rewards) + plt.plot(q) + plt.show() + #""" + rollout = (q[0], self.random_state.rand(), self.params, q, self.cov) + heapq.heappush(self.best_rollouts, rollout) + + if self.log_to_stdout or self.log_to_file: + self.logger.info("Iteration #%d, return: %g" % (self.it, q[0])) + self.logger.info("Variance: %g" % np.mean(self.cov)) + + self.it += 1 + if self.it % self.n_samples_per_update == 0: + self._update_weights() + self._update_variance() + + def _update_variance(self): + if len(self.best_rollouts) < 2: + return + + # We use more rollouts for the variance calculation to avoid + # rapid convergence to 0 + var_nom = np.zeros_like(self.last_mean) + var_dnom = 0.0 + for _, _, params, q, _ in heapq.nlargest(30, self.best_rollouts): + # This simplified version of the update assumes + # * that the covariance is a diagonal matrix + # * noise is the same over a whole rollout + q_sum = np.sum(q) + var_nom += q_sum * (params - self.last_mean) ** 2 + var_dnom += q_sum + # TODO without division by 10 the variance grows too fast, any idea? + self.cov = var_nom / (10 * var_dnom + 1e-10) + # apply and an upper and a lower limit to the exploration + #self.cov = np.clip(var_nom / (var_dnom + 1e-10), 0.1 * self.initial_cov, 10.0 * self.initial_cov) + + def _update_weights(self): + self.last_mean[:] = self.mean + + n_features = self.dmp_behavior.n_features + n_task_dims = self.dmp_behavior.n_task_dims + + best_rollouts = heapq.nlargest( + self.n_samples_per_update, self.best_rollouts) + + param_nom = np.empty(n_features) + param_dnom = np.empty((n_features, n_features)) + for d in range(n_task_dims): + lo = d * n_features + hi = (1 + d) * n_features + param_nom[:] = 0 + param_dnom[:, :] = 0 + for _, _, params, q, cov in best_rollouts: + cov_d = cov[lo:hi] + W = np.array([self.tmp_outer[i] / + self.basis[i].T.dot(cov_d * self.basis[i]) + for i in range(self.basis.shape[0])]) + epsilon = params[lo:hi] - self.mean[lo:hi] + + param_nom += np.sum(W.dot(epsilon) * q[:, np.newaxis], axis=0) + param_dnom += np.sum(W * q[:, np.newaxis, np.newaxis], axis=0) + + inv_param_dnom = np.linalg.pinv(param_dnom) + self.mean[lo:hi] += param_nom.dot(inv_param_dnom) + + def is_behavior_learning_done(self): + """Check if the behavior learning is finished, e.g. it converged. + + Returns + ------- + finished : bool + Is the learning of a behavior finished? + """ + return False + + def get_best_behavior(self): + """Returns the best behavior found so far. + + Returns + ------- + behavior : Behavior + mapping from input to output + """ + best_rollout = heapq.nlargest(1, self.best_rollouts)[0] + self.dmp_behavior.set_params(best_rollout[2]) + self.dmp_behavior.reset() + return self.dmp_behavior diff --git a/bolero/optimizer/cmaes.py b/bolero/optimizer/cmaes.py index dd355549..2f173223 100644 --- a/bolero/optimizer/cmaes.py +++ b/bolero/optimizer/cmaes.py @@ -142,7 +142,7 @@ def init(self, n_params): self.eigen_decomp_updated = 0 if self.initial_params is None: - self.initial_params = np.zeros(n_params) + self.initial_params = np.zeros(self.n_params) else: self.initial_params = np.asarray(self.initial_params).astype( np.float64, copy=True) diff --git a/bolero/representation/promp_behavior.py b/bolero/representation/promp_behavior.py index 43f0a546..500c0689 100644 --- a/bolero/representation/promp_behavior.py +++ b/bolero/representation/promp_behavior.py @@ -110,7 +110,6 @@ def __init__(self, execution_time=1.0, dt=0.01, n_features=50, - overlap=0.7, configuration_file=None, learn_covariance=False, use_covar=False): @@ -289,7 +288,7 @@ def get_n_params(self): random_variables = len(self.data.mean_) if self.learn_covariance: correlation_coefficients = ( - len(self.data.covariance_) - random_variables) / 2 + len(self.data.covariance_) - random_variables) // 2 return 2 * random_variables + correlation_coefficients else: return random_variables diff --git a/examples/behavior_search/plot_obstacle_avoidance_power.py b/examples/behavior_search/plot_obstacle_avoidance_power.py new file mode 100644 index 00000000..0071be66 --- /dev/null +++ b/examples/behavior_search/plot_obstacle_avoidance_power.py @@ -0,0 +1,70 @@ +""" +====================== +Obstacle Avoidance DMP +====================== + +We use PoWER to optimize a DMP so that it avoids point obstacles. +""" +print(__doc__) + +import numpy as np +import matplotlib.pyplot as plt +from bolero.environment import OptimumTrajectory +from bolero.behavior_search import PoWERWithDMP +from bolero.representation import DMPBehavior +from bolero.controller import Controller + + +n_task_dims = 2 +obstacles = [np.array([0.5, 0.5]), np.array([0.6, 0.8]), np.array([0.8, 0.6])] +x0 = np.zeros(n_task_dims) +g = np.ones(n_task_dims) +execution_time = 1.0 +dt = 0.01 +n_features = 6 +n_episodes = 1000 +reward_transformation = lambda r: np.exp(0.001 * r) + +beh = DMPBehavior(execution_time, dt, n_features) +env = OptimumTrajectory( + x0, + g, + execution_time, + dt, + obstacles, + penalty_goal_dist=1.0, + penalty_obstacle=1000.0, + penalty_acc=1.0) +bs = PoWERWithDMP( + beh, variance=100.0 ** 2, reward_transformation=reward_transformation, + random_state=0, log_to_stdout=True) +controller = Controller( + environment=env, + behavior_search=bs, + n_episodes=n_episodes, + record_inputs=True, + verbose=2 +) + +rewards = controller.learn(["x0", "g"], [x0, g]) +controller.episode_with(bs.get_best_behavior(), ["x0", "g"], [x0, g]) +X = np.asarray(controller.inputs_[-1]) +X_hist = np.asarray(controller.inputs_) + +plt.figure(figsize=(8, 5)) +ax = plt.subplot(121) +ax.set_title("Optimization progress") +ax.plot(rewards) +ax.set_xlabel("Episode") +ax.set_ylabel("Reward") + +ax = plt.subplot(122, aspect="equal") +ax.set_title("Learned trajectory") +env.plot(ax) +ax.plot(X[:, 0], X[:, 1], lw=5, label="Final trajectory") +for it, X in enumerate(X_hist[::int(n_episodes / 10)]): + ax.plot(X[:, 0], X[:, 1], c="k", alpha=it / 20.0, lw=3, ls="--") +ax.set_xticks(()) +ax.set_yticks(()) +plt.legend(loc="best") +plt.show() diff --git a/src/representation/dmp/implementation/dmp/_declarations.pxd b/src/representation/dmp/implementation/dmp/_declarations.pxd index e7408d55..2300efcf 100644 --- a/src/representation/dmp/implementation/dmp/_declarations.pxd +++ b/src/representation/dmp/implementation/dmp/_declarations.pxd @@ -1,6 +1,9 @@ from libcpp cimport bool +cdef extern from "../src/Dmp.h" namespace "Dmp": + double phase(double t, double alpha, double goal_t, double start_t) except + + cdef extern from "../src/Dmp.h" namespace "Dmp": double calculateAlpha(double goal_z, double goal_t, double start_t) except + diff --git a/src/representation/dmp/implementation/dmp/dmp.pyx b/src/representation/dmp/implementation/dmp/dmp.pyx index 3ec98ed9..4584c71b 100644 --- a/src/representation/dmp/implementation/dmp/dmp.pyx +++ b/src/representation/dmp/implementation/dmp/dmp.pyx @@ -4,10 +4,28 @@ import numpy as np cimport _declarations as cpp +cpdef phase(double t, double alpha, double goal_t, double start_t): + """Determine phase value that corresponds to the current time in the DMP. + + \param t current time, note that t is allowed to be outside of the range + [start_t, goal_t] + \param alpha constant that defines the decay rate of the phase variable + \param goal_t time at the end of the DMP + \param start_t time at the start of the DMP + \return phase value (z) + """ + cdef double cpp_t = t + cdef double cpp_alpha = alpha + cdef double cpp_goal_t = goal_t + cdef double cpp_start_t = start_t + cdef double result = cpp.phase(cpp_t, cpp_alpha, cpp_goal_t, cpp_start_t) + return result + + cpdef calculate_alpha(double goal_z, double goal_t, double start_t): """Compute decay rate of phase variable so that a desired phase is reached in the end. - + \param goal_z desired phase value \param goal_t time at the end of the DMP \param start_t time at the start of the DMP @@ -18,9 +36,10 @@ cpdef calculate_alpha(double goal_z, double goal_t, double start_t): cdef double result = cpp.calculateAlpha(cpp_goal_z, cpp_goal_t, cpp_start_t) return result + cpdef initialize_rbf(np.ndarray[double, ndim=1] widths, np.ndarray[double, ndim=1] centers, double goal_t, double start_t, double overlap, double alpha): """Initialize radial basis functions. - + \param widths widths of the RBFs, will be initialized \param num_widths number of RBFs \param centers centers of the RBFs, will be initialized @@ -38,14 +57,14 @@ cpdef initialize_rbf(np.ndarray[double, ndim=1] widths, np.ndarray[double, ndim= cpdef imitate(np.ndarray[double, ndim=1] T, np.ndarray[double, ndim=2] Y, np.ndarray[double, ndim=2] weights, np.ndarray[double, ndim=1] widths, np.ndarray[double, ndim=1] centers, double regularization_coefficient, double alpha_y, double beta_y, double alpha_z, bool allow_final_velocity): """Represent trajectory as DMP. - + \note The final velocity will be calculated by numeric differentiation from the data if allow_final_velocity is true. Otherwise we will assume the final velocity to be zero. To reproduce the trajectory as closely as possible, set the initial acceleration and velocity during execution to zero, the final acceleration to zero and the final velocity to the value that has been used during imitation. - + \param T time for each step of the trajectory \param num_T number of steps \param Y positions, contains num_T * num_dimensions entries in row-major @@ -78,9 +97,9 @@ cpdef imitate(np.ndarray[double, ndim=1] T, np.ndarray[double, ndim=2] Y, np.nda cpdef dmp_step(double last_t, double t, np.ndarray[double, ndim=1] last_y, np.ndarray[double, ndim=1] last_yd, np.ndarray[double, ndim=1] last_ydd, np.ndarray[double, ndim=1] y, np.ndarray[double, ndim=1] yd, np.ndarray[double, ndim=1] ydd, np.ndarray[double, ndim=1] goal_y, np.ndarray[double, ndim=1] goal_yd, np.ndarray[double, ndim=1] goal_ydd, np.ndarray[double, ndim=1] start_y, np.ndarray[double, ndim=1] start_yd, np.ndarray[double, ndim=1] start_ydd, double goal_t, double start_t, np.ndarray[double, ndim=2] weights, np.ndarray[double, ndim=1] widths, np.ndarray[double, ndim=1] centers, double alpha_y, double beta_y, double alpha_z, double integration_dt): """Execute one step of the DMP. - + source: http://ijr.sagepub.com/content/32/3/263.full.pdf - + \param last_t time of last step (should equal t initially) \param t current time \param last_y last position @@ -136,14 +155,14 @@ cpdef dmp_step(double last_t, double t, np.ndarray[double, ndim=1] last_y, np.nd cpdef quaternion_imitate(np.ndarray[double, ndim=1] T, np.ndarray[double, ndim=2] R, np.ndarray[double, ndim=2] weights, np.ndarray[double, ndim=1] widths, np.ndarray[double, ndim=1] centers, double regularization_coefficient, double alpha_r, double beta_r, double alpha_z, bool allow_final_velocity): """Represent trajectory as quaternion DMP. - + \note The final velocity will be calculated by numeric differentiation from the data if allow_final_velocity is true. Otherwise we will assume the final velocity to be zero. To reproduce the trajectory as closely as possible, set the initial acceleration and velocity during execution to zero, the final acceleration to zero and the final velocity to the value that has been used during imitation. - + \param T time for each step of the trajectory \param num_T number of steps \param R rotations, contains num_T * 4 entries in row-major order, i.e. @@ -175,9 +194,9 @@ cpdef quaternion_imitate(np.ndarray[double, ndim=1] T, np.ndarray[double, ndim=2 cpdef quaternion_dmp_step(double last_t, double t, np.ndarray[double, ndim=1] last_r, np.ndarray[double, ndim=1] last_rd, np.ndarray[double, ndim=1] last_rdd, np.ndarray[double, ndim=1] r, np.ndarray[double, ndim=1] rd, np.ndarray[double, ndim=1] rdd, np.ndarray[double, ndim=1] goal_r, np.ndarray[double, ndim=1] goal_rd, np.ndarray[double, ndim=1] goal_rdd, np.ndarray[double, ndim=1] start_r, np.ndarray[double, ndim=1] start_rd, np.ndarray[double, ndim=1] start_rdd, double goal_t, double start_t, np.ndarray[double, ndim=2] weights, np.ndarray[double, ndim=1] widths, np.ndarray[double, ndim=1] centers, double alpha_r, double beta_r, double alpha_z, double integration_dt): """Execute one step of the Quaternion DMP. - + source: http://ieeexplore.ieee.org/document/6907291/?arnumber=6907291 - + \param last_t time of last step (should equal t initially) \param t current time \param last_r last rotation diff --git a/src/representation/dmp/implementation/src/Dmp.cpp b/src/representation/dmp/implementation/src/Dmp.cpp index 7b010a1d..c9511b36 100644 --- a/src/representation/dmp/implementation/src/Dmp.cpp +++ b/src/representation/dmp/implementation/src/Dmp.cpp @@ -11,22 +11,6 @@ namespace Dmp { -/** - * Determine phase value that corresponds to the current time in the DMP. - * \param t current time, note that t is allowed to be outside of the range - * [start_t, goal_t] - * \param alpha constant that defines the decay rate of the phase variable - * \param goal_t time at the end of the DMP - * \param start_t time at the start of the DMP - * \return phase value (z) - */ -const double phase( - const double t, - const double alpha, - const double goal_t, - const double start_t -); - /** * Calculates the gradient function for \p in, e.g. the derivation. * The returned gradient has the same shape as the input array. diff --git a/src/representation/dmp/implementation/src/Dmp.h b/src/representation/dmp/implementation/src/Dmp.h index 6218e3d7..be8697c5 100644 --- a/src/representation/dmp/implementation/src/Dmp.h +++ b/src/representation/dmp/implementation/src/Dmp.h @@ -2,6 +2,22 @@ namespace Dmp { +/** + * Determine phase value that corresponds to the current time in the DMP. + * \param t current time, note that t is allowed to be outside of the range + * [start_t, goal_t] + * \param alpha constant that defines the decay rate of the phase variable + * \param goal_t time at the end of the DMP + * \param start_t time at the start of the DMP + * \return phase value (z) + */ +const double phase( + const double t, + const double alpha, + const double goal_t, + const double start_t +); + /** * Compute decay rate of phase variable so that a desired phase is reached in * the end.