def test_quaternion_step_invalid_times(): last_r = np.array([0.0, 1.0, 0.0, 0.0]) last_rd = np.array([0.0, 0.0, 0.0]) last_rdd = np.array([0.0, 0.0, 0.0]) r = np.array([0.0, 1.0, 0.0, 0.0]) rd = np.array([0.0, 0.0, 0.0]) rdd = np.array([0.0, 0.0, 0.0]) g = np.array([0.0, 0.0, 7.07106781e-01, 7.07106781e-01]) gd = np.array([0.0, 0.0, 0.0]) gdd = np.array([0.0, 0.0, 0.0]) r0 = np.array([0.0, 1.0, 0.0, 0.0]) r0d = np.array([0.0, 0.0, 0.0]) r0dd = np.array([0.0, 0.0, 0.0]) n_features = 10 weights = np.zeros((n_features, 3)) execution_time = 1.0 alpha = 25.0 widths = np.empty(n_features) centers = np.empty(n_features) dmp.initialize_rbf(widths, centers, execution_time, 0.0, 0.8, alpha / 3.0) assert_raises_regexp(ValueError, "Goal must be chronologically after start", dmp.quaternion_dmp_step, 0.0, 0.1, last_r, last_rd, last_rdd, r, rd, rdd, g, gd, gdd, r0, r0d, r0dd, 0.0, 0.0, weights, widths, centers, alpha, alpha / 4.0, alpha / 3.0, 0.001)
def test_step_invalid_times(): last_y = np.array([0.0]) last_yd = np.array([0.0]) last_ydd = np.array([0.0]) y = np.empty([1]) yd = np.empty([1]) ydd = np.empty([1]) g = np.array([1.0]) gd = np.array([0.0]) gdd = np.array([0.0]) n_weights = 10 weights = np.zeros((n_weights, 1)) execution_time = 1.0 alpha = 25.0 widths = np.empty(n_weights) centers = np.empty(n_weights) dmp.initialize_rbf(widths, centers, execution_time, 0.0, 0.8, alpha / 3.0) assert_raises_regexp(ValueError, "Goal must be chronologically after start", dmp.dmp_step, 0.0, 0.1, last_y, last_yd, last_ydd, y, yd, ydd, g, gd, gdd, np.array([0.0]), np.array([0.0]), np.array([0.0]), 0.0, 0.0, weights, widths, centers, alpha, alpha / 4.0, alpha / 3.0, 0.001)
def imitate(T, Y, n_features, alpha): widths = np.empty(n_features) centers = np.empty(n_features) weights = np.empty(n_features * 2) dmp.initialize_rbf(widths, centers, 1.0, 0.8, 25.0 / 3.0) dmp.imitate(T, Y.ravel(), weights, widths, centers, 1e-10, alpha, alpha / 4.0, alpha / 3.0, False) return widths, centers, weights
def test_quaternion_imitate(): T = np.linspace(0, 2, 101) n_features = 20 alpha = 25.0 widths = np.empty(n_features) centers = np.empty(n_features) dmp.initialize_rbf(widths, centers, T[-1], T[0], 0.8, alpha / 3.0) R = np.empty((T.shape[0], 4)) for i in range(T.shape[0]): angle = T[i] * np.pi R[i] = np.array([ np.cos(angle / 2.0), np.sqrt(0.5) * np.sin(angle / 2.0), np.sqrt(0.5) * np.sin(angle / 2.0), 0.0 ]) weights = np.empty((n_features, 3)) dmp.quaternion_imitate(T, R, weights, widths, centers, 1e-10, alpha, alpha / 4.0, alpha / 3.0, False) last_t = T[0] last_r = R[0].copy() last_rd = np.zeros(3) last_rdd = np.zeros(3) r = R[0].copy() rd = np.zeros(3) rdd = np.zeros(3) g = R[-1].copy() gd = np.zeros(3) gdd = np.zeros(3) r0 = R[0].copy() r0d = np.zeros(3) r0dd = np.zeros(3) R_replay = [] for t in T: dmp.quaternion_dmp_step(last_t, t, last_r, last_rd, last_rdd, r, rd, rdd, g, gd, gdd, r0, r0d, r0dd, T[-1], T[0], weights, widths, centers, alpha, alpha / 4.0, alpha / 3.0, 0.001) last_t = t last_r[:] = r last_rd[:] = rd last_rdd[:] = rdd R_replay.append(r.copy()) R_replay = np.asarray(R_replay) distances = np.array( [np.linalg.norm(r - r_replay) for r, r_replay in zip(R, R_replay)]) assert_less(distances.max(), 0.043) assert_less(distances.min(), 1e-10) assert_less(sorted(distances)[len(distances) // 2], 0.02) assert_less(np.mean(distances), 0.02)
def test_imitate(): T = np.linspace(0, 2, 101) n_features = 9 widths = np.empty(n_features) centers = np.empty(n_features) dmp.initialize_rbf(widths, centers, T[-1], T[0], 0.8, 25.0 / 3.0) Y = np.hstack((T[:, np.newaxis], np.cos(np.pi * T)[:, np.newaxis])) weights = np.empty((n_features, 2)) alpha = 25.0 dmp.imitate(T, Y, weights, widths, centers, 1e-10, alpha, alpha / 4.0, alpha / 3.0, False) last_t = T[0] last_y = Y[0].copy() last_yd = np.zeros(2) last_ydd = np.zeros(2) y = Y[0].copy() yd = np.zeros(2) ydd = np.zeros(2) g = Y[-1].copy() gd = np.zeros(2) gdd = np.zeros(2) y0 = Y[0].copy() y0d = np.zeros(2) y0dd = np.zeros(2) Y_replay = [] for t in np.linspace(T[0], T[-1], T.shape[0]): dmp.dmp_step(last_t, t, last_y, last_yd, last_ydd, y, yd, ydd, g, gd, gdd, y0, y0d, y0dd, T[-1], T[0], weights, widths, centers, alpha, alpha / 4.0, alpha / 3.0, 0.001) last_t = t last_y[:] = y last_yd[:] = yd last_ydd[:] = ydd Y_replay.append(y.copy()) Y_replay = np.asarray(Y_replay) distances = np.array( [np.linalg.norm(y - y_replay) for y, y_replay in zip(Y, Y_replay)]) assert_less(distances.max(), 0.032) assert_less(distances.min(), 1e-10) assert_less(sorted(distances)[len(distances) // 2], 0.02) assert_less(np.mean(distances), 0.02)
def test_imitate_ill_conditioning(): n_features = 101 widths = np.empty(n_features) centers = np.empty(n_features) dmp.initialize_rbf(widths, centers, 1.0, 0.0, 0.8, 25.0 / 3.0) T = np.linspace(0, 1, 101) Y = np.hstack((T[:, np.newaxis], np.cos(2 * np.pi * T)[:, np.newaxis])) weights = np.empty((n_features, 2)) alpha = 25.0 assert_raises_regexp(ValueError, "must be >= 0", dmp.imitate, T, Y, weights, widths, centers, -1.0, alpha, alpha / 4.0, alpha / 3.0, False) assert_raises_regexp(ValueError, "instable", dmp.imitate, T, Y, weights, widths, centers, 0.0, alpha, alpha / 4.0, alpha / 3.0, False)
def init(self, n_inputs, n_outputs): """Initialize the behavior. Parameters ---------- n_inputs : int number of inputs n_outputs : int number of outputs """ if n_inputs != n_outputs: raise ValueError("Input and output dimensions must match, got " "%d inputs and %d outputs" % (n_inputs, n_outputs)) self.n_inputs = n_inputs self.n_outputs = n_outputs self.n_task_dims = self.n_inputs / 3 if hasattr(self, "configuration_file"): load_dmp_model(self, self.configuration_file) else: self.name = "Python DMP" self.alpha_z = dmp.calculate_alpha(0.01, self.execution_time, 0.0) self.widths = np.empty(self.n_features) self.centers = np.empty(self.n_features) dmp.initialize_rbf(self.widths, self.centers, self.execution_time, 0.0, 0.8, self.alpha_z) self.alpha_y = 25.0 self.beta_y = self.alpha_y / 4.0 self.weights = np.zeros((self.n_features, self.n_task_dims)) if not hasattr(self, "x0"): self.x0 = None if not hasattr(self, "x0d"): self.x0d = np.zeros(self.n_task_dims) if not hasattr(self, "x0dd"): self.x0dd = np.zeros(self.n_task_dims) if not hasattr(self, "g"): self.g = np.zeros(self.n_task_dims) if not hasattr(self, "gd"): self.gd = np.zeros(self.n_task_dims) if not hasattr(self, "gdd"): self.gdd = np.zeros(self.n_task_dims) self.reset()
def test_quaternion_step(): last_r = np.array([0.0, 1.0, 0.0, 0.0]) last_rd = np.array([0.0, 0.0, 0.0]) last_rdd = np.array([0.0, 0.0, 0.0]) r = np.array([0.0, 1.0, 0.0, 0.0]) rd = np.array([0.0, 0.0, 0.0]) rdd = np.array([0.0, 0.0, 0.0]) g = np.array([0.0, 0.0, 7.07106781e-01, 7.07106781e-01]) gd = np.array([0.0, 0.0, 0.0]) gdd = np.array([0.0, 0.0, 0.0]) r0 = np.array([0.0, 1.0, 0.0, 0.0]) r0d = np.array([0.0, 0.0, 0.0]) r0dd = np.array([0.0, 0.0, 0.0]) n_features = 10 weights = np.zeros((n_features, 3)) execution_time = 1.0 alpha = 25.0 widths = np.empty(n_features) centers = np.empty(n_features) dmp.initialize_rbf(widths, centers, execution_time, 0.0, 0.8, alpha / 3.0) T = np.linspace(0.0, 1.0 * execution_time, 1001) last_t = 0.0 R_replay = [] for t in T: dmp.quaternion_dmp_step(last_t, t, last_r, last_rd, last_rdd, r, rd, rdd, g, gd, gdd, r0, r0d, r0dd, execution_time, 0.0, weights, widths, centers, alpha, alpha / 4.0, alpha / 3.0, 0.001) last_t = t last_r[:] = r last_rd[:] = rd last_rdd[:] = rdd R_replay.append(r.copy()) R_replay = np.asarray(R_replay) assert_array_almost_equal(r, g, decimal=4) assert_array_almost_equal(rd, gd, decimal=3) assert_array_almost_equal(rdd, gdd, decimal=2)
def test_initialize_rbf_backward_compatibility(): widths = np.empty(10) centers = np.empty(10) dmp.initialize_rbf(widths, centers, 0.446, 0.0, 0.8, 4.5814764780043) assert_array_almost_equal( widths, np.array([ 1.39105769528669, 3.87070066903258, 10.7704545397461, 29.969429545615, 83.3917179609352, 232.042408878407, 645.671786535403, 1796.62010036397, 4999.20215246208, 4999.20215246208 ])) assert_array_almost_equal( centers, np.array([ 1, 0.59948425031894, 0.359381366380461, 0.215443469003187, 0.129154966501487, 0.0774263682681117, 0.0464158883361271, 0.0278255940220708, 0.0166810053720003, 0.00999999999999978 ]))
def test_step(): last_y = np.array([0.0]) last_yd = np.array([0.0]) last_ydd = np.array([0.0]) y = np.empty([1]) yd = np.empty([1]) ydd = np.empty([1]) g = np.array([1.0]) gd = np.array([0.0]) gdd = np.array([0.0]) n_weights = 10 weights = np.zeros((n_weights, 1)) execution_time = 1.0 alpha = 25.0 widths = np.empty(n_weights) centers = np.empty(n_weights) dmp.initialize_rbf(widths, centers, execution_time, 0.0, 0.8, alpha / 3.0) last_t = 0.0 # Execute DMP longer than expected duration for t in np.linspace(0.0, 1.5 * execution_time, 151): dmp.dmp_step(last_t, t, last_y, last_yd, last_ydd, y, yd, ydd, g, gd, gdd, np.array([0.0]), np.array([0.0]), np.array([0.0]), execution_time, 0.0, weights, widths, centers, alpha, alpha / 4.0, alpha / 3.0, 0.001) last_t = t last_y[:] = y last_yd[:] = yd last_ydd[:] = ydd assert_array_almost_equal(y, g, decimal=6) assert_array_almost_equal(yd, gd, decimal=5) assert_array_almost_equal(ydd, gdd, decimal=4)
def init(self, n_inputs, n_outputs): """Initialize the behavior. Parameters ---------- n_inputs : int number of inputs n_outputs : int number of outputs """ if n_inputs != n_outputs: raise ValueError("Input and output dimensions must match, got " "%d inputs and %d outputs" % (n_inputs, n_outputs)) self.n_task_dims = n_inputs // 3 if self.execution_times is None: self.execution_times = np.ones(self.n_dmps) self.execution_times = np.asarray(self.execution_times) if self.n_features is None: self.n_features = 50 * np.ones(self.n_dmps, dtype=int) if self.subgoals is None: self.subgoals = [ np.zeros(self.n_task_dims) for _ in range(self.n_dmps + 1) ] else: self.subgoals = [np.asarray(g) for g in self.subgoals] self.n_steps_per_dmp = (self.execution_times / self.dt).astype(int) + 1 self.n_weights_per_dmp = self.n_task_dims * self.n_features self.subgoal_velocities = [ np.zeros(self.n_task_dims) for _ in range(self.n_dmps + 1) ] self.n_weights = np.sum(self.n_weights_per_dmp) self.split_steps = np.cumsum(self.n_steps_per_dmp - 1) self.split_weights = np.cumsum(self.n_weights_per_dmp) self.alpha_z = [] self.widths = [] self.centers = [] for i in range(self.n_dmps): alpha_z = dmp.calculate_alpha(0.01, self.execution_times[i], 0.0) widths = np.empty(self.n_features[i]) centers = np.empty(self.n_features[i]) dmp.initialize_rbf(widths, centers, self.execution_times[i], 0.0, 0.8, alpha_z) self.alpha_z.append(alpha_z) self.widths.append(widths) self.centers.append(centers) self.alpha_y = 25.0 self.beta_y = self.alpha_y / 4.0 if self.initial_weights is None: self.weights = [ np.zeros((self.n_weights_per_dmp[i], self.n_task_dims)) for i in range(self.n_dmps) ] else: self.weights = [ w.reshape(self.n_weights_per_dmp[i], self.n_task_dims) for w in self.initial_weights ] self.x0 = None self.g = None self.reset()
def init(self, n_inputs, n_outputs): """Initialize the behavior. Parameters ---------- n_inputs : int number of inputs n_outputs : int number of outputs """ if n_inputs != 7: raise ValueError("Number of inputs must be 7") if n_outputs != 7: raise ValueError("Number of outputs must be 7") self.n_inputs = 7 self.n_outputs = 7 self.n_task_dims = 6 if hasattr(self, "configuration_file"): load_dmp_model(self, self.configuration_file) else: self.name = "Python CSDMP" self.alpha_z = dmp.calculate_alpha(0.01, self.execution_time, 0.0) self.widths = np.empty(self.n_features) self.centers = np.empty(self.n_features) dmp.initialize_rbf(self.widths, self.centers, self.execution_time, 0.0, 0.8, self.alpha_z) self.alpha_y = 25.0 self.beta_y = self.alpha_y / 4.0 self.position_weights = np.empty((self.n_features, 3)) self.orientation_weights = np.empty((self.n_features, 3)) self.weights = np.zeros((self.n_features, self.n_task_dims)) if not hasattr(self, "x0"): self.x0 = np.zeros(3) if not hasattr(self, "x0d"): self.x0d = np.zeros(3) if not hasattr(self, "x0dd"): self.x0dd = np.zeros(3) if not hasattr(self, "g"): self.g = np.zeros(3) if not hasattr(self, "gd"): self.gd = np.zeros(3) if not hasattr(self, "gdd"): self.gdd = np.zeros(3) if not hasattr(self, "q0"): self.q0 = np.array([0.0, 1.0, 0.0, 0.0]) if not hasattr(self, "q0d"): self.q0d = np.zeros(3) if not hasattr(self, "q0dd"): self.q0dd = np.zeros(3) if not hasattr(self, "qg"): self.qg = np.array([0.0, 1.0, 0.0, 0.0]) if not hasattr(self, "qgd"): self.qgd = np.zeros(3) if not hasattr(self, "qgdd"): self.qgdd = np.zeros(3) self.reset()