From fc56a30bf606a72bf3bdad4559c22f191d6d474a Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Tue, 29 Jun 2021 11:19:02 -0500 Subject: [PATCH 01/52] Without equals sign, sometimes get points that are in the wrong direction - relative to the points before and after it- when change in x or change in y along path is 0 --- PathPlanning/DubinsPath/dubins_path_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 9c13198aff..440abba4da 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -255,7 +255,7 @@ def generate_local_course(total_length, lengths, mode, max_curvature, ll = 0.0 for (m, l, i) in zip(mode, lengths, range(len(mode))): - if l > 0.0: + if l >= 0.0: d = step_size else: d = -step_size From 64ca35ea8e518e6e9f25b238a428aa7084abc23c Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Tue, 29 Jun 2021 11:27:21 -0500 Subject: [PATCH 02/52] Created test script for dubins path generator --- tests/test_dubins.py | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 tests/test_dubins.py diff --git a/tests/test_dubins.py b/tests/test_dubins.py new file mode 100644 index 0000000000..e145658d65 --- /dev/null +++ b/tests/test_dubins.py @@ -0,0 +1,11 @@ +import conftest +from PathPlanning.DubinsPath import dubins_path_planning as m + + +def test_1(): + m.show_animation = True + m.main() + + +if __name__ == '__main__': + conftest.run_this_test(__file__) From e02ec7d52396a757a55dcf25a2183cbb4d276088 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 30 Jun 2021 09:12:08 -0500 Subject: [PATCH 03/52] Made len == 0 it's own case, also changed 'l' to 'len' to appease travisCI --- PathPlanning/DubinsPath/dubins_path_planning.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 9c13198aff..41f815e0cf 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -254,8 +254,10 @@ def generate_local_course(total_length, lengths, mode, max_curvature, ll = 0.0 - for (m, l, i) in zip(mode, lengths, range(len(mode))): - if l > 0.0: + for (mode, len, i) in zip(mode, lengths, range(len(mode))): + if len == 0: + continue + elif len > 0.0: d = step_size else: d = -step_size @@ -273,15 +275,15 @@ def generate_local_course(total_length, lengths, mode, max_curvature, while abs(pd) <= abs(l): index += 1 path_x, path_y, path_yaw, directions = interpolate( - index, pd, m, max_curvature, origin_x, origin_y, origin_yaw, + index, pd, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) pd += d - ll = l - pd - d # calc remain length + ll = len - pd - d # calc remain length index += 1 path_x, path_y, path_yaw, directions = interpolate( - index, l, m, max_curvature, origin_x, origin_y, origin_yaw, + index, len, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) if len(path_x) <= 1: From 80991d0178bc5ff5b65429340952cab9512ff0e7 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 30 Jun 2021 10:11:46 -0500 Subject: [PATCH 04/52] More variable renaming to appease CI --- PathPlanning/DubinsPath/dubins_path_planning.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 41f815e0cf..f5e9482fea 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -258,9 +258,9 @@ def generate_local_course(total_length, lengths, mode, max_curvature, if len == 0: continue elif len > 0.0: - d = step_size + dist = step_size else: - d = -step_size + dist = -step_size # set origin state origin_x, origin_y, origin_yaw = \ @@ -268,18 +268,18 @@ def generate_local_course(total_length, lengths, mode, max_curvature, index -= 1 if i >= 1 and (lengths[i - 1] * lengths[i]) > 0: - pd = - d - ll + pd = - dist - ll else: - pd = d - ll + pd = dist - ll while abs(pd) <= abs(l): index += 1 path_x, path_y, path_yaw, directions = interpolate( index, pd, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) - pd += d + pd += dist - ll = len - pd - d # calc remain length + ll = len - pd - dist # calc remain length index += 1 path_x, path_y, path_yaw, directions = interpolate( From f6b99fd7ada51c8f2760c0290f106dfb83d2f6b3 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 30 Jun 2021 10:14:22 -0500 Subject: [PATCH 05/52] Broke == 0 into its own case in dubins planner, also Renaming files to appease CI --- .../DubinsPath/dubins_path_planning.py | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 440abba4da..f5e9482fea 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -254,11 +254,13 @@ def generate_local_course(total_length, lengths, mode, max_curvature, ll = 0.0 - for (m, l, i) in zip(mode, lengths, range(len(mode))): - if l >= 0.0: - d = step_size + for (mode, len, i) in zip(mode, lengths, range(len(mode))): + if len == 0: + continue + elif len > 0.0: + dist = step_size else: - d = -step_size + dist = -step_size # set origin state origin_x, origin_y, origin_yaw = \ @@ -266,22 +268,22 @@ def generate_local_course(total_length, lengths, mode, max_curvature, index -= 1 if i >= 1 and (lengths[i - 1] * lengths[i]) > 0: - pd = - d - ll + pd = - dist - ll else: - pd = d - ll + pd = dist - ll while abs(pd) <= abs(l): index += 1 path_x, path_y, path_yaw, directions = interpolate( - index, pd, m, max_curvature, origin_x, origin_y, origin_yaw, + index, pd, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) - pd += d + pd += dist - ll = l - pd - d # calc remain length + ll = len - pd - dist # calc remain length index += 1 path_x, path_y, path_yaw, directions = interpolate( - index, l, m, max_curvature, origin_x, origin_y, origin_yaw, + index, len, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) if len(path_x) <= 1: From eddba93ccd5115ff0caf451e355c800b0c0045c7 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 30 Jun 2021 10:19:48 -0500 Subject: [PATCH 06/52] Reverting some naming changes --- PathPlanning/DubinsPath/dubins_path_planning.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index f5e9482fea..f849f59010 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -254,10 +254,10 @@ def generate_local_course(total_length, lengths, mode, max_curvature, ll = 0.0 - for (mode, len, i) in zip(mode, lengths, range(len(mode))): - if len == 0: + for (m, length, i) in zip(mode, lengths, range(len(mode))): + if length == 0: continue - elif len > 0.0: + elif length > 0.0: dist = step_size else: dist = -step_size @@ -275,15 +275,15 @@ def generate_local_course(total_length, lengths, mode, max_curvature, while abs(pd) <= abs(l): index += 1 path_x, path_y, path_yaw, directions = interpolate( - index, pd, mode, max_curvature, origin_x, origin_y, origin_yaw, + index, pd, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) pd += dist - ll = len - pd - dist # calc remain length + ll = length - pd - dist # calc remain length index += 1 path_x, path_y, path_yaw, directions = interpolate( - index, len, mode, max_curvature, origin_x, origin_y, origin_yaw, + index, length, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) if len(path_x) <= 1: From 30df7e856945169c5c778c3b6a2349d143f0df4a Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 30 Jun 2021 10:35:46 -0500 Subject: [PATCH 07/52] Turns out theres already a test for dubins.. not sure how I missed that --- tests/test_dubins.py | 11 ----------- 1 file changed, 11 deletions(-) delete mode 100644 tests/test_dubins.py diff --git a/tests/test_dubins.py b/tests/test_dubins.py deleted file mode 100644 index e145658d65..0000000000 --- a/tests/test_dubins.py +++ /dev/null @@ -1,11 +0,0 @@ -import conftest -from PathPlanning.DubinsPath import dubins_path_planning as m - - -def test_1(): - m.show_animation = True - m.main() - - -if __name__ == '__main__': - conftest.run_this_test(__file__) From 59778694342089f12994ff6ac733c1effd91509d Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 30 Jun 2021 10:46:13 -0500 Subject: [PATCH 08/52] Note to self: run the test cases on your own before throwing them at CI --- PathPlanning/DubinsPath/dubins_path_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index f849f59010..64d29e5061 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -272,7 +272,7 @@ def generate_local_course(total_length, lengths, mode, max_curvature, else: pd = dist - ll - while abs(pd) <= abs(l): + while abs(pd) <= abs(length): index += 1 path_x, path_y, path_yaw, directions = interpolate( index, pd, m, max_curvature, origin_x, origin_y, origin_yaw, From ba86afd7525e85d5a7269d11e35aef478ad27afd Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 1 Jul 2021 10:14:36 -0500 Subject: [PATCH 09/52] Added handling of length=0 case in generate_local_course() --- .../ReedsSheppPath/reeds_shepp_path_planning.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 7808f2c9f9..c7d2b209ce 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -285,8 +285,10 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size) ll = 0.0 - for (m, l, i) in zip(mode, lengths, range(len(mode))): - if l > 0.0: + for (m, length, i) in zip(mode, lengths, range(len(mode))): + if length == 0.0: + continue + elif length > 0.0: d = step_size else: d = -step_size @@ -300,17 +302,17 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size) else: pd = d - ll - while abs(pd) <= abs(l): + while abs(pd) <= abs(length): ind += 1 px, py, pyaw, directions = interpolate( ind, pd, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions) pd += d - ll = l - pd - d # calc remain length + ll = length - pd - d # calc remain length ind += 1 px, py, pyaw, directions = interpolate( - ind, l, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions) + ind, length, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions) # remove unused data while px[-1] == 0.0: From c0d3d3624f536de5e65383467b5aa6a624dfbae6 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 1 Jul 2021 10:24:58 -0500 Subject: [PATCH 10/52] Missed reverting 'mode' back to 'm' in one spot --- PathPlanning/DubinsPath/dubins_path_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index b123b13704..a813d79b0d 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -275,7 +275,7 @@ def generate_local_course(total_length, lengths, mode, max_curvature, while abs(pd) <= abs(length): index += 1 path_x, path_y, path_yaw, directions = interpolate( - index, pd, mode, max_curvature, origin_x, origin_y, origin_yaw, + index, pd, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) pd += dist From 831c2aa8171166f90868a7c597f4b8e193509f94 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 1 Jul 2021 10:35:58 -0500 Subject: [PATCH 11/52] Addressing style issues (line length) --- PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index c7d2b209ce..0b31e73793 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -305,14 +305,16 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size) while abs(pd) <= abs(length): ind += 1 px, py, pyaw, directions = interpolate( - ind, pd, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions) + ind, pd, m, max_curvature, ox, oy, oyaw, + px, py, pyaw, directions) pd += d ll = length - pd - d # calc remain length ind += 1 px, py, pyaw, directions = interpolate( - ind, length, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions) + ind, length, m, max_curvature, ox, oy, oyaw, + px, py, pyaw, directions) # remove unused data while px[-1] == 0.0: @@ -392,7 +394,8 @@ def main(): step_size = 0.1 px, py, pyaw, mode, clen = reeds_shepp_path_planning( - start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size) + start_x, start_y, start_yaw, end_x, end_y, end_yaw, + curvature, step_size) if show_animation: # pragma: no cover plt.cla() From d5b4361f251adacb8eca528a2a23399a9aebbd1d Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 7 Jul 2021 11:51:15 -0500 Subject: [PATCH 12/52] Mostly works, now just need to setup linear regression to solve for weights --- .../dynamic_movement_primitives.py | 85 +++++++++++++++++++ 1 file changed, 85 insertions(+) create mode 100644 ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py diff --git a/ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py b/ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py new file mode 100644 index 0000000000..27242a2803 --- /dev/null +++ b/ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py @@ -0,0 +1,85 @@ +import matplotlib.pyplot as plt +import numpy as np + +class DMP(object): + + def __init__(self, K=156.25, B=25, dt=0.001, timesteps=5000, training_data=None): + self.K = K # virtual spring constant + self.B = B # virtual damper coefficient + + self.dt = dt + self.timesteps = timesteps + + if training_data is None: + self.training_data = [] + self.train_t_vals = np.linspace(0, np.pi, 250) + + for t in self.train_t_vals[:-125]: + self.training_data.append(np.random.normal(np.sin(t), 0.02)) + for t in self.train_t_vals[125:]: + self.training_data.append(np.random.normal(self.training_data[-1], 0.005)) + + dt = np.max(self.train_t_vals)/(self.train_t_vals.size - 1) + self.find_basis_functions_weights(training_data, dt) + else: + raise NotImplementedError + + def find_basis_functions_weights(self, data, dt): + # need to do linear regression to get w + + self.w = [-275, 405.7, -535, 264, -111] + + return 0 + + def recreate_trajectory(self, q0, g, T): + ''' + q0 - initial state/position + g - goal state/position + T - amount of time to travek q0 -> g + ''' + + nrBasis = len(self.w) # number of gaussian basis functions + + # means (C) and std devs (H) of gaussian basis functions + C = np.linspace(0,1,nrBasis) + H = (0.65*(1./(nrBasis-1))**2) + + # initialize virtual system + q = q0 + qd = 0 + qdd = 0 + t = 0 + + positions = [] + for k in range(self.timesteps): + t = t + self.dt + if t <=T: + Phi = [np.exp(-0.5 * ((t/T)-c)**2 / H) for c in C] + Phi = Phi/np.sum(Phi) + f = np.dot(Phi, self.w) + else: + f=0 + + # simulate dynamics + qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 + qd = qd + qdd * self.dt + q = q + qd * self.dt + positions.append(q) + + return np.arange(0,self.timesteps * self.dt, self.dt), positions + + def solve_trajectory(self, q0, g, T): + + t, pos = self.recreate_trajectory(q0, g, T) + + plt.plot(self.train_t_vals, self.training_data, label="Training Data") + plt.plot(t,pos,label="DMP Approximation") + plt.xlabel("Time [s]") + plt.ylabel("Position [m]") + plt.legend() + plt.show() + + +if __name__ == '__main__': + DMP_controller = DMP() + DMP_controller.solve_trajectory(-1, 3,5) From e3f9a9d2fcfe0f5b7e310de88d76b7ef92d77ee5 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 7 Jul 2021 14:19:41 -0500 Subject: [PATCH 13/52] Re-arranged class --- .../dynamic_movement_primitives.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py b/ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py index 27242a2803..546e0509c5 100644 --- a/ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py +++ b/ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py @@ -17,7 +17,7 @@ def __init__(self, K=156.25, B=25, dt=0.001, timesteps=5000, training_data=None) for t in self.train_t_vals[:-125]: self.training_data.append(np.random.normal(np.sin(t), 0.02)) for t in self.train_t_vals[125:]: - self.training_data.append(np.random.normal(self.training_data[-1], 0.005)) + self.training_data.append(self.training_data[-1]) dt = np.max(self.train_t_vals)/(self.train_t_vals.size - 1) self.find_basis_functions_weights(training_data, dt) @@ -26,6 +26,8 @@ def __init__(self, K=156.25, B=25, dt=0.001, timesteps=5000, training_data=None) def find_basis_functions_weights(self, data, dt): # need to do linear regression to get w + for i in range(len(data)): + print(i) self.w = [-275, 405.7, -535, 264, -111] From dd94aaf9bf08b08f08b3bbf1783db341625a3d79 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 8 Jul 2021 11:21:23 -0500 Subject: [PATCH 14/52] Wrote DMP program and added tests file --- .../dynamic_movement_primitives.py | 87 ---------- .../dynamic_movement_primitives.py | 157 ++++++++++++++++++ tests/test_dynamic_movement_primitives.py | 32 ++++ 3 files changed, 189 insertions(+), 87 deletions(-) delete mode 100644 ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py create mode 100644 ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py create mode 100644 tests/test_dynamic_movement_primitives.py diff --git a/ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py b/ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py deleted file mode 100644 index 546e0509c5..0000000000 --- a/ArmNavigation/Dynamic_Movement_Primatives/dynamic_movement_primitives.py +++ /dev/null @@ -1,87 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np - -class DMP(object): - - def __init__(self, K=156.25, B=25, dt=0.001, timesteps=5000, training_data=None): - self.K = K # virtual spring constant - self.B = B # virtual damper coefficient - - self.dt = dt - self.timesteps = timesteps - - if training_data is None: - self.training_data = [] - self.train_t_vals = np.linspace(0, np.pi, 250) - - for t in self.train_t_vals[:-125]: - self.training_data.append(np.random.normal(np.sin(t), 0.02)) - for t in self.train_t_vals[125:]: - self.training_data.append(self.training_data[-1]) - - dt = np.max(self.train_t_vals)/(self.train_t_vals.size - 1) - self.find_basis_functions_weights(training_data, dt) - else: - raise NotImplementedError - - def find_basis_functions_weights(self, data, dt): - # need to do linear regression to get w - for i in range(len(data)): - print(i) - - self.w = [-275, 405.7, -535, 264, -111] - - return 0 - - def recreate_trajectory(self, q0, g, T): - ''' - q0 - initial state/position - g - goal state/position - T - amount of time to travek q0 -> g - ''' - - nrBasis = len(self.w) # number of gaussian basis functions - - # means (C) and std devs (H) of gaussian basis functions - C = np.linspace(0,1,nrBasis) - H = (0.65*(1./(nrBasis-1))**2) - - # initialize virtual system - q = q0 - qd = 0 - qdd = 0 - t = 0 - - positions = [] - for k in range(self.timesteps): - t = t + self.dt - if t <=T: - Phi = [np.exp(-0.5 * ((t/T)-c)**2 / H) for c in C] - Phi = Phi/np.sum(Phi) - f = np.dot(Phi, self.w) - else: - f=0 - - # simulate dynamics - qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 - qd = qd + qdd * self.dt - q = q + qd * self.dt - positions.append(q) - - return np.arange(0,self.timesteps * self.dt, self.dt), positions - - def solve_trajectory(self, q0, g, T): - - t, pos = self.recreate_trajectory(q0, g, T) - - plt.plot(self.train_t_vals, self.training_data, label="Training Data") - plt.plot(t,pos,label="DMP Approximation") - plt.xlabel("Time [s]") - plt.ylabel("Position [m]") - plt.legend() - plt.show() - - -if __name__ == '__main__': - DMP_controller = DMP() - DMP_controller.solve_trajectory(-1, 3,5) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py new file mode 100644 index 0000000000..ed13ef8191 --- /dev/null +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -0,0 +1,157 @@ +''' +Author: Jonathan Schwartz (github.com/SchwartzCode) +More information on Dynamic Movement Primitives available at: +https://arxiv.org/abs/2102.03861 +https://www.frontiersin.org/articles/10.3389/fncom.2013.00138/full +''' +import matplotlib.pyplot as plt +import numpy as np + +class DMP(object): + + def __init__(self, K=156.25, B=25, dt=0.001, timesteps=5000, training_data=None): + self.K = K # virtual spring constant + self.B = B # virtual damper coefficient + + self.dt = dt + self.timesteps = timesteps + + if training_data is None: + self.training_data = [] + self.train_t_vals = np.linspace(0, np.pi, 150) + + for t in self.train_t_vals[:125]: + self.training_data.append(np.random.normal(np.sin(t), 0.02)) + for t in self.train_t_vals[125:]: + self.training_data.append(self.training_data[-1]) + + dt = np.max(self.train_t_vals)/(self.train_t_vals.size - 1) + else: + self.train_t_vals = training_data[0] + self.training_data = training_data[1] + dt = self.train_t_vals[1] - self.train_t_vals[0] + + self.find_basis_functions_weights(self.training_data, dt) + + def find_basis_functions_weights(self, data, dt, num_weights=5): + + # means (C) and std devs (H) of gaussian basis functions + C = np.linspace(0,1,num_weights) + H = (0.65*(1./(num_weights-1))**2) + + q0 = data[0] # initial pos + g = data[-1] # assume goal is reached by end of data + self.T_orig = len(data)*dt # time duration of data + + q = q0 + qd_last = 0 + + phi_vals = [] + f_vals = [] + + for i in range(len(data)): + if i+1 == len(data): + qd = 0 + else: + qd = (data[i+1] - data[i])/dt + + Phi = [np.exp(-0.5 * ((i*dt/self.T_orig)-c)**2 / H) for c in C] + Phi = Phi/np.sum(Phi) + + qdd = (qd - qd_last)/dt + + f = (qdd * self.T_orig**2 - self.K*(g - q) + self.B*qd*self.T_orig) / (g - q0) + + phi_vals.append(Phi) + f_vals.append(f) + + qd_last = qd + q += qd*dt + + phi_vals = np.asarray(phi_vals) + f_vals = np.asarray(f_vals) + + print(phi_vals.shape, f_vals.shape) + w = np.linalg.lstsq(phi_vals, f_vals, rcond=None) + self.w = w[0] + + return w[0] + + def recreate_trajectory(self, q0, g, T): + ''' + q0 - initial state/position + g - goal state/position + T - amount of time to travek q0 -> g + ''' + + nrBasis = len(self.w) # number of gaussian basis functions + + # means (C) and std devs (H) of gaussian basis functions + C = np.linspace(0,1,nrBasis) + H = (0.65*(1./(nrBasis-1))**2) + + # initialize virtual system + q = q0 + qd = 0 + qdd = 0 + t = 0 + + positions = [] + for k in range(self.timesteps): + t = t + self.dt + if t <=T: + Phi = [np.exp(-0.5 * ((t/T)-c)**2 / H) for c in C] + Phi = Phi/np.sum(Phi) + f = np.dot(Phi, self.w) + else: + f=0 + + # simulate dynamics + qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 + qd = qd + qdd * self.dt + q = q + qd * self.dt + positions.append(q) + + return np.arange(0,self.timesteps * self.dt, self.dt), positions + + def solve_trajectory(self, q0, g, T, visualize=True): + + t, pos = self.recreate_trajectory(q0, g, T) + + if visualize: + plt.plot(self.train_t_vals, self.training_data, label="Training Data") + plt.plot(t,pos,label="DMP Approximation") + plt.xlabel("Time [s]") + plt.ylabel("Position [m]") + plt.legend() + plt.show() + + return t, pos + + def show_DMP_purpose(self): + ''' + This function conveys the purpose of DMPs: + to capture a trajectory and be able to stretch + and squeeze it in terms of start and stop position + or time + ''' + q0_orig = self.training_data[0] + g_orig = self.training_data[-1] + + t_norm, pos_norm = self.recreate_trajectory(q0_orig, g_orig, self.T_orig) + t_fast, pos_fast = self.recreate_trajectory(q0_orig, g_orig, self.T_orig/2) + t_close, pos_close = self.recreate_trajectory(q0_orig, g_orig/2, self.T_orig) + + plt.plot(self.train_t_vals, self.training_data, label="Training Data") + plt.plot(t_norm,pos_norm,label="DMP Approximation", linestyle='--') + plt.plot(t_fast, pos_fast, label="Decreasing time duration", linestyle='--') + plt.plot(t_close, pos_close, label='Decreasing goal position', linestyle='--') + plt.xlabel("Time [s]") + plt.ylabel("Position [m]") + plt.legend() + plt.show() + +if __name__ == '__main__': + DMP_controller = DMP() + # DMP_controller.show_DMP_purpose() + # DMP_controller.solve_trajectory(0, 1, 3) diff --git a/tests/test_dynamic_movement_primitives.py b/tests/test_dynamic_movement_primitives.py new file mode 100644 index 0000000000..511350078f --- /dev/null +++ b/tests/test_dynamic_movement_primitives.py @@ -0,0 +1,32 @@ +import conftest +import numpy as np +from ArmNavigation.dynamic_movement_primitives import dynamic_movement_primitives + +def test_1(): + # test that functions work at all + DMP_controller = dynamic_movement_primitives.DMP() + DMP_controller.solve_trajectory(0,3,3, visualize=False) + +def test_2(): + # test that trajectory can be learned from user-passed data + t = np.arange(0,5,0.01) + sin_t = np.sin(t) + + train_data = [t,sin_t] + DMP_controller = dynamic_movement_primitives.DMP(training_data=train_data) + DMP_controller.solve_trajectory(-3,3,4, visualize=False) + +def test_3(): + # check that learned trajectory is close to initial + t = np.arange(0,3*np.pi/2,0.01) + sin_t = np.sin(t) + + train_data = [t,sin_t] + DMP_controller = dynamic_movement_primitives.DMP(dt=0.01, timesteps=len(t), training_data=train_data) + t, pos = DMP_controller.solve_trajectory(0,-1,3*np.pi/2, visualize=False) + + diff = abs(pos - sin_t) + assert(max(diff) < 0.1) + +if __name__ == '__main__': + conftest.run_this_test(__file__) From 732560b0e398fdc8e23653a2fb425ee55d392307 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 8 Jul 2021 11:44:43 -0500 Subject: [PATCH 15/52] Styling fixes --- .../dynamic_movement_primitives.py | 58 +++++++++++-------- tests/test_dynamic_movement_primitives.py | 23 +++++--- 2 files changed, 50 insertions(+), 31 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index ed13ef8191..740e0d68f3 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -7,9 +7,11 @@ import matplotlib.pyplot as plt import numpy as np + class DMP(object): - def __init__(self, K=156.25, B=25, dt=0.001, timesteps=5000, training_data=None): + def __init__(self, K=156.25, B=25, dt=0.001, timesteps=5000, + training_data=None): self.K = K # virtual spring constant self.B = B # virtual damper coefficient @@ -36,11 +38,11 @@ def __init__(self, K=156.25, B=25, dt=0.001, timesteps=5000, training_data=None) def find_basis_functions_weights(self, data, dt, num_weights=5): # means (C) and std devs (H) of gaussian basis functions - C = np.linspace(0,1,num_weights) + C = np.linspace(0, 1, num_weights) H = (0.65*(1./(num_weights-1))**2) q0 = data[0] # initial pos - g = data[-1] # assume goal is reached by end of data + g = data[-1] # assume goal is reached by end of data self.T_orig = len(data)*dt # time duration of data q = q0 @@ -49,29 +51,30 @@ def find_basis_functions_weights(self, data, dt, num_weights=5): phi_vals = [] f_vals = [] - for i in range(len(data)): - if i+1 == len(data): + for i, _ in enumerate(data): + if i + 1 == len(data): qd = 0 else: - qd = (data[i+1] - data[i])/dt + qd = (data[i+1] - data[i]) / dt - Phi = [np.exp(-0.5 * ((i*dt/self.T_orig)-c)**2 / H) for c in C] + Phi = [np.exp(-0.5 * ((i * dt / self.T_orig) - c)**2 / H) + for c in C] Phi = Phi/np.sum(Phi) qdd = (qd - qd_last)/dt - f = (qdd * self.T_orig**2 - self.K*(g - q) + self.B*qd*self.T_orig) / (g - q0) + f = (qdd * self.T_orig**2 - self.K * (g - q) + self.B * qd + * self.T_orig) / (g - q0) phi_vals.append(Phi) f_vals.append(f) qd_last = qd - q += qd*dt + q += qd * dt phi_vals = np.asarray(phi_vals) f_vals = np.asarray(f_vals) - print(phi_vals.shape, f_vals.shape) w = np.linalg.lstsq(phi_vals, f_vals, rcond=None) self.w = w[0] @@ -99,9 +102,9 @@ def recreate_trajectory(self, q0, g, T): positions = [] for k in range(self.timesteps): t = t + self.dt - if t <=T: - Phi = [np.exp(-0.5 * ((t/T)-c)**2 / H) for c in C] - Phi = Phi/np.sum(Phi) + if t <= T: + Phi = [np.exp(-0.5 * ((t / T) - c)**2 / H) for c in C] + Phi = Phi / np.sum(Phi) f = np.dot(Phi, self.w) else: f=0 @@ -119,7 +122,8 @@ def solve_trajectory(self, q0, g, T, visualize=True): t, pos = self.recreate_trajectory(q0, g, T) if visualize: - plt.plot(self.train_t_vals, self.training_data, label="Training Data") + plt.plot(self.train_t_vals, self.training_data, + label="Training Data") plt.plot(t,pos,label="DMP Approximation") plt.xlabel("Time [s]") plt.ylabel("Position [m]") @@ -138,20 +142,28 @@ def show_DMP_purpose(self): q0_orig = self.training_data[0] g_orig = self.training_data[-1] - t_norm, pos_norm = self.recreate_trajectory(q0_orig, g_orig, self.T_orig) - t_fast, pos_fast = self.recreate_trajectory(q0_orig, g_orig, self.T_orig/2) - t_close, pos_close = self.recreate_trajectory(q0_orig, g_orig/2, self.T_orig) - - plt.plot(self.train_t_vals, self.training_data, label="Training Data") - plt.plot(t_norm,pos_norm,label="DMP Approximation", linestyle='--') - plt.plot(t_fast, pos_fast, label="Decreasing time duration", linestyle='--') - plt.plot(t_close, pos_close, label='Decreasing goal position', linestyle='--') + t_norm, pos_norm = self.recreate_trajectory(q0_orig, + g_orig, self.T_orig) + t_fast, pos_fast = self.recreate_trajectory(q0_orig, + g_orig, self.T_orig/2) + t_close, pos_close = self.recreate_trajectory(q0_orig, + g_orig/2, self.T_orig) + + plt.plot(self.train_t_vals, self.training_data, + label="Training Data") + plt.plot(t_norm,pos_norm,label="DMP Approximation", + linestyle='--') + plt.plot(t_fast, pos_fast, label="Decreasing time duration", + linestyle='--') + plt.plot(t_close, pos_close, label='Decreasing goal position', + linestyle='--') plt.xlabel("Time [s]") plt.ylabel("Position [m]") plt.legend() plt.show() + if __name__ == '__main__': DMP_controller = DMP() - # DMP_controller.show_DMP_purpose() + DMP_controller.show_DMP_purpose() # DMP_controller.solve_trajectory(0, 1, 3) diff --git a/tests/test_dynamic_movement_primitives.py b/tests/test_dynamic_movement_primitives.py index 511350078f..b41ec3a1bc 100644 --- a/tests/test_dynamic_movement_primitives.py +++ b/tests/test_dynamic_movement_primitives.py @@ -1,32 +1,39 @@ import conftest import numpy as np -from ArmNavigation.dynamic_movement_primitives import dynamic_movement_primitives +from ArmNavigation.dynamic_movement_primitives import \ + dynamic_movement_primitives + def test_1(): # test that functions work at all DMP_controller = dynamic_movement_primitives.DMP() - DMP_controller.solve_trajectory(0,3,3, visualize=False) + DMP_controller.solve_trajectory(0, 3, 3, visualize=False) + def test_2(): # test that trajectory can be learned from user-passed data - t = np.arange(0,5,0.01) + t = np.arange(0, 5, 0.01) sin_t = np.sin(t) train_data = [t,sin_t] DMP_controller = dynamic_movement_primitives.DMP(training_data=train_data) - DMP_controller.solve_trajectory(-3,3,4, visualize=False) + DMP_controller.solve_trajectory(-3, 3, 4, visualize=False) + def test_3(): # check that learned trajectory is close to initial - t = np.arange(0,3*np.pi/2,0.01) + t = np.arange(0, 3*np.pi/2, 0.01) sin_t = np.sin(t) - train_data = [t,sin_t] - DMP_controller = dynamic_movement_primitives.DMP(dt=0.01, timesteps=len(t), training_data=train_data) - t, pos = DMP_controller.solve_trajectory(0,-1,3*np.pi/2, visualize=False) + train_data = [t, sin_t] + DMP_controller = dynamic_movement_primitives.DMP(dt=0.01, + timesteps=len(t), training_data=train_data) + t, pos = DMP_controller.solve_trajectory(0, -1, 3*np.pi/2, + visualize=False) diff = abs(pos - sin_t) assert(max(diff) < 0.1) + if __name__ == '__main__': conftest.run_this_test(__file__) From a7b5e254c3236009736a3fae6aafdd292ee8d7e0 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 8 Jul 2021 11:54:10 -0500 Subject: [PATCH 16/52] More styling --- .../dynamic_movement_primitives.py | 37 ++++++++++--------- tests/test_dynamic_movement_primitives.py | 7 ++-- 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 740e0d68f3..70bcda4454 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -11,7 +11,7 @@ class DMP(object): def __init__(self, K=156.25, B=25, dt=0.001, timesteps=5000, - training_data=None): + training_data=None): self.K = K # virtual spring constant self.B = B # virtual damper coefficient @@ -42,7 +42,7 @@ def find_basis_functions_weights(self, data, dt, num_weights=5): H = (0.65*(1./(num_weights-1))**2) q0 = data[0] # initial pos - g = data[-1] # assume goal is reached by end of data + g = data[-1] # assume goal is reached by end of data self.T_orig = len(data)*dt # time duration of data q = q0 @@ -58,13 +58,13 @@ def find_basis_functions_weights(self, data, dt, num_weights=5): qd = (data[i+1] - data[i]) / dt Phi = [np.exp(-0.5 * ((i * dt / self.T_orig) - c)**2 / H) - for c in C] + for c in C] Phi = Phi/np.sum(Phi) qdd = (qd - qd_last)/dt f = (qdd * self.T_orig**2 - self.K * (g - q) + self.B * qd - * self.T_orig) / (g - q0) + * self.T_orig) / (g - q0) phi_vals.append(Phi) f_vals.append(f) @@ -90,7 +90,7 @@ def recreate_trajectory(self, q0, g, T): nrBasis = len(self.w) # number of gaussian basis functions # means (C) and std devs (H) of gaussian basis functions - C = np.linspace(0,1,nrBasis) + C = np.linspace(0, 1, nrBasis) H = (0.65*(1./(nrBasis-1))**2) # initialize virtual system @@ -107,7 +107,7 @@ def recreate_trajectory(self, q0, g, T): Phi = Phi / np.sum(Phi) f = np.dot(Phi, self.w) else: - f=0 + f = 0 # simulate dynamics qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 @@ -115,7 +115,7 @@ def recreate_trajectory(self, q0, g, T): q = q + qd * self.dt positions.append(q) - return np.arange(0,self.timesteps * self.dt, self.dt), positions + return np.arange(0, self.timesteps * self.dt, self.dt), positions def solve_trajectory(self, q0, g, T, visualize=True): @@ -123,8 +123,8 @@ def solve_trajectory(self, q0, g, T, visualize=True): if visualize: plt.plot(self.train_t_vals, self.training_data, - label="Training Data") - plt.plot(t,pos,label="DMP Approximation") + label="Training Data") + plt.plot(t, pos, label="DMP Approximation") plt.xlabel("Time [s]") plt.ylabel("Position [m]") plt.legend() @@ -143,20 +143,23 @@ def show_DMP_purpose(self): g_orig = self.training_data[-1] t_norm, pos_norm = self.recreate_trajectory(q0_orig, - g_orig, self.T_orig) + g_orig, + self.T_orig) t_fast, pos_fast = self.recreate_trajectory(q0_orig, - g_orig, self.T_orig/2) + g_orig, + self.T_orig/2) t_close, pos_close = self.recreate_trajectory(q0_orig, - g_orig/2, self.T_orig) + g_orig/2, + self.T_orig) plt.plot(self.train_t_vals, self.training_data, - label="Training Data") - plt.plot(t_norm,pos_norm,label="DMP Approximation", - linestyle='--') + label="Training Data") + plt.plot(t_norm, pos_norm, label="DMP Approximation", + linestyle='--') plt.plot(t_fast, pos_fast, label="Decreasing time duration", - linestyle='--') + linestyle='--') plt.plot(t_close, pos_close, label='Decreasing goal position', - linestyle='--') + linestyle='--') plt.xlabel("Time [s]") plt.ylabel("Position [m]") plt.legend() diff --git a/tests/test_dynamic_movement_primitives.py b/tests/test_dynamic_movement_primitives.py index b41ec3a1bc..bb4f337e2d 100644 --- a/tests/test_dynamic_movement_primitives.py +++ b/tests/test_dynamic_movement_primitives.py @@ -15,7 +15,7 @@ def test_2(): t = np.arange(0, 5, 0.01) sin_t = np.sin(t) - train_data = [t,sin_t] + train_data = [t, sin_t] DMP_controller = dynamic_movement_primitives.DMP(training_data=train_data) DMP_controller.solve_trajectory(-3, 3, 4, visualize=False) @@ -27,9 +27,10 @@ def test_3(): train_data = [t, sin_t] DMP_controller = dynamic_movement_primitives.DMP(dt=0.01, - timesteps=len(t), training_data=train_data) + timesteps=len(t), + training_data=train_data) t, pos = DMP_controller.solve_trajectory(0, -1, 3*np.pi/2, - visualize=False) + visualize=False) diff = abs(pos - sin_t) assert(max(diff) < 0.1) From c2ca3f063a10bbac685020e1cb9bf43c822b984f Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 8 Jul 2021 11:57:55 -0500 Subject: [PATCH 17/52] Missed one indent --- .../dynamic_movement_primitives/dynamic_movement_primitives.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 70bcda4454..0a68685313 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -64,7 +64,7 @@ def find_basis_functions_weights(self, data, dt, num_weights=5): qdd = (qd - qd_last)/dt f = (qdd * self.T_orig**2 - self.K * (g - q) + self.B * qd - * self.T_orig) / (g - q0) + * self.T_orig) / (g - q0) phi_vals.append(Phi) f_vals.append(f) From 4372e74e0b0eede9db74c7981c8b282309931aed Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 18 Jul 2021 19:19:20 -0400 Subject: [PATCH 18/52] Multi-dimension path learning (e.g. in x and y instead of just x) --- .../dynamic_movement_primitives.py | 191 +++++++++++------- 1 file changed, 119 insertions(+), 72 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 0a68685313..4d7a7c70ce 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -1,119 +1,157 @@ -''' +""" Author: Jonathan Schwartz (github.com/SchwartzCode) More information on Dynamic Movement Primitives available at: https://arxiv.org/abs/2102.03861 https://www.frontiersin.org/articles/10.3389/fncom.2013.00138/full -''' +""" import matplotlib.pyplot as plt import numpy as np class DMP(object): - def __init__(self, K=156.25, B=25, dt=0.001, timesteps=5000, - training_data=None): + def __init__(self, training_data, data_period, K=156.25, B=25, dt=0.001, + timesteps=5000): + """ + Arguments: + training_data - data in for [(x1,y1), (x2,y2), ...] + data_period - amount of time training data covers + K and B - spring and damper constants to define DMP behavior + dt - timestep between points in generated trajectories + timesteps - number of points in generated trajectories + """ self.K = K # virtual spring constant self.B = B # virtual damper coefficient self.dt = dt self.timesteps = timesteps - if training_data is None: - self.training_data = [] - self.train_t_vals = np.linspace(0, np.pi, 150) + self.weights = None # weights used to generate DMP trajectories - for t in self.train_t_vals[:125]: - self.training_data.append(np.random.normal(np.sin(t), 0.02)) - for t in self.train_t_vals[125:]: - self.training_data.append(self.training_data[-1]) + # self.T_orig = data_period + # self.training_data = training_data - dt = np.max(self.train_t_vals)/(self.train_t_vals.size - 1) - else: - self.train_t_vals = training_data[0] - self.training_data = training_data[1] - dt = self.train_t_vals[1] - self.train_t_vals[0] + self.find_basis_functions_weights(training_data, data_period) - self.find_basis_functions_weights(self.training_data, dt) + def find_basis_functions_weights(self, training_data, data_period, num_weights=5): + """ + Arguments: + data [(steps x spacial dim) np array] - data to replicate with DMP + data_period [float] - time duration of data + """ - def find_basis_functions_weights(self, data, dt, num_weights=5): + self.training_data = training_data # for plotting + + dt = data_period / len(training_data) + + init_state = training_data[0] # initial pos + goal_state = training_data[-1] # assume goal is reached by end of data # means (C) and std devs (H) of gaussian basis functions C = np.linspace(0, 1, num_weights) H = (0.65*(1./(num_weights-1))**2) - q0 = data[0] # initial pos - g = data[-1] # assume goal is reached by end of data - self.T_orig = len(data)*dt # time duration of data + for dim in range(len(training_data[0])): - q = q0 - qd_last = 0 + dimension_data = training_data[:,dim] - phi_vals = [] - f_vals = [] + q0 = init_state[dim] + g = goal_state[dim] - for i, _ in enumerate(data): - if i + 1 == len(data): - qd = 0 - else: - qd = (data[i+1] - data[i]) / dt + q = q0 + qd_last = 0 + + phi_vals = [] + f_vals = [] + + #TODO: fix dimensions + for i, _ in enumerate(dimension_data): + if i + 1 == len(dimension_data): + qd = 0 + else: + qd = (dimension_data[i+1] - dimension_data[i]) / dt - Phi = [np.exp(-0.5 * ((i * dt / self.T_orig) - c)**2 / H) - for c in C] - Phi = Phi/np.sum(Phi) + Phi = [np.exp(-0.5 * ((i * dt / data_period) - c)**2 / H) + for c in C] + Phi = Phi/np.sum(Phi) - qdd = (qd - qd_last)/dt + qdd = (qd - qd_last)/dt - f = (qdd * self.T_orig**2 - self.K * (g - q) + self.B * qd - * self.T_orig) / (g - q0) + f = (qdd * data_period**2 - self.K * (g - q) + self.B * qd + * data_period) / (g - q0) - phi_vals.append(Phi) - f_vals.append(f) + phi_vals.append(Phi) + f_vals.append(f) - qd_last = qd - q += qd * dt + qd_last = qd + q += qd * dt - phi_vals = np.asarray(phi_vals) - f_vals = np.asarray(f_vals) + phi_vals = np.asarray(phi_vals) + f_vals = np.asarray(f_vals) - w = np.linalg.lstsq(phi_vals, f_vals, rcond=None) - self.w = w[0] + w = np.linalg.lstsq(phi_vals, f_vals, rcond=None) - return w[0] + if self.weights is None: + self.weights = np.asarray(w[0]) + else: + self.weights = np.vstack([self.weights, w[0]]) + + return self.weights # TODO: neccesary? - def recreate_trajectory(self, q0, g, T): - ''' - q0 - initial state/position - g - goal state/position + def recreate_trajectory(self, init_state, goal_state, T): + """ + init_state - initial state/position + goal_state - goal state/position T - amount of time to travek q0 -> g - ''' + """ - nrBasis = len(self.w) # number of gaussian basis functions + nrBasis = len(self.weights[0]) # number of gaussian basis functions # means (C) and std devs (H) of gaussian basis functions C = np.linspace(0, 1, nrBasis) H = (0.65*(1./(nrBasis-1))**2) # initialize virtual system - q = q0 - qd = 0 - qdd = 0 + state = init_state + state_dot = [0,0] + state_double_dot = [0,0] t = 0 + self.train_t_vals = np.arange(0, T, self.timesteps) + # TODO: is self.period variable used + positions = [] for k in range(self.timesteps): + new_state = [] t = t + self.dt - if t <= T: - Phi = [np.exp(-0.5 * ((t / T) - c)**2 / H) for c in C] - Phi = Phi / np.sum(Phi) - f = np.dot(Phi, self.w) - else: - f = 0 - # simulate dynamics - qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 - qd = qd + qdd * self.dt - q = q + qd * self.dt - positions.append(q) + for dim in range(self.weights.shape[0]): + + q0 = init_state[dim] + q = state[dim] + qd = state_dot[dim] + qdd = state_double_dot[dim] + g = goal_state[dim] + + if t <= T: + Phi = [np.exp(-0.5 * ((t / T) - c)**2 / H) for c in C] + Phi = Phi / np.sum(Phi) + f = np.dot(Phi, self.weights[dim]) + else: + f = 0 + + # simulate dynamics + qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 + qd = qd + qdd * self.dt + q = q + qd * self.dt + + state_dot[dim] = qd + state_double_dot[dim] = qdd + + new_state.append(q) + + state = new_state + positions.append(new_state) return np.arange(0, self.timesteps * self.dt, self.dt), positions @@ -121,24 +159,29 @@ def solve_trajectory(self, q0, g, T, visualize=True): t, pos = self.recreate_trajectory(q0, g, T) + state_hist = np.asarray(pos) + if visualize: - plt.plot(self.train_t_vals, self.training_data, + plt.plot(self.training_data[:,0], self.training_data[:,1], label="Training Data") - plt.plot(t, pos, label="DMP Approximation") + plt.plot(state_hist[:,0], state_hist[:,1], + label="DMP Approximation") plt.xlabel("Time [s]") plt.ylabel("Position [m]") plt.legend() plt.show() + + return t, pos def show_DMP_purpose(self): - ''' + """ This function conveys the purpose of DMPs: to capture a trajectory and be able to stretch and squeeze it in terms of start and stop position or time - ''' + """ q0_orig = self.training_data[0] g_orig = self.training_data[-1] @@ -167,6 +210,10 @@ def show_DMP_purpose(self): if __name__ == '__main__': - DMP_controller = DMP() - DMP_controller.show_DMP_purpose() - # DMP_controller.solve_trajectory(0, 1, 3) + period = np.pi / 2 + t = np.arange(0, period, 0.01) + training_data = np.asarray([np.sin(t), np.cos(t)]).T + + DMP_controller = DMP(training_data, period) + # DMP_controller.show_DMP_purpose() + DMP_controller.solve_trajectory([0,1], [1,0], 3) From 3ca74cbd5eccb84b904ec6e4167c36af0c28eb28 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 18 Jul 2021 19:51:17 -0400 Subject: [PATCH 19/52] Added potential field obstacle avoidance --- .../dynamic_movement_primitives.py | 69 ++++++++++++++----- 1 file changed, 51 insertions(+), 18 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 4d7a7c70ce..c649c58d7c 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -11,7 +11,7 @@ class DMP(object): def __init__(self, training_data, data_period, K=156.25, B=25, dt=0.001, - timesteps=5000): + timesteps=5000, repel_factor=1): """ Arguments: training_data - data in for [(x1,y1), (x2,y2), ...] @@ -19,6 +19,7 @@ def __init__(self, training_data, data_period, K=156.25, B=25, dt=0.001, K and B - spring and damper constants to define DMP behavior dt - timestep between points in generated trajectories timesteps - number of points in generated trajectories + repel_factor - controls how much path will avoid obstacles """ self.K = K # virtual spring constant self.B = B # virtual damper coefficient @@ -28,6 +29,8 @@ def __init__(self, training_data, data_period, K=156.25, B=25, dt=0.001, self.weights = None # weights used to generate DMP trajectories + self.repel_factor = repel_factor + # self.T_orig = data_period # self.training_data = training_data @@ -64,7 +67,6 @@ def find_basis_functions_weights(self, training_data, data_period, num_weights=5 phi_vals = [] f_vals = [] - #TODO: fix dimensions for i, _ in enumerate(dimension_data): if i + 1 == len(dimension_data): qd = 0 @@ -117,6 +119,7 @@ def recreate_trajectory(self, init_state, goal_state, T): state_double_dot = [0,0] t = 0 + # for plotting self.train_t_vals = np.arange(0, T, self.timesteps) # TODO: is self.period variable used @@ -125,6 +128,10 @@ def recreate_trajectory(self, init_state, goal_state, T): new_state = [] t = t + self.dt + obs_force = 0 + if self.obstacles is not None: + obs_force = self.get_obstacle_force(state) + for dim in range(self.weights.shape[0]): q0 = init_state[dim] @@ -141,7 +148,8 @@ def recreate_trajectory(self, init_state, goal_state, T): f = 0 # simulate dynamics - qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 + qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 \ + + obs_force[dim] qd = qd + qdd * self.dt q = q + qd * self.dt @@ -155,23 +163,45 @@ def recreate_trajectory(self, init_state, goal_state, T): return np.arange(0, self.timesteps * self.dt, self.dt), positions - def solve_trajectory(self, q0, g, T, visualize=True): + def get_obstacle_force(self, state): + + obstacle_force = np.asarray([0.0, 0.0]) + + for obs in self.obstacles: + new_force = [] + + dist = np.sum(np.sqrt((obs - state)**2)) + force = self.repel_factor * dist + + # TODO: all lists or all np arrays for inputs + for dim in range(len(self.obstacles[0])): + new_force.append(force * (state[dim] - obs[dim]) / dist) + + obstacle_force += np.array(new_force) + + return obstacle_force + + def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None): + + self.obstacles = obstacles t, pos = self.recreate_trajectory(q0, g, T) state_hist = np.asarray(pos) if visualize: - plt.plot(self.training_data[:,0], self.training_data[:,1], - label="Training Data") - plt.plot(state_hist[:,0], state_hist[:,1], - label="DMP Approximation") - plt.xlabel("Time [s]") - plt.ylabel("Position [m]") - plt.legend() - plt.show() - - + if self.training_data.shape[1] == 2: + if self.obstacles is not None: + for obs in self.obstacles: + plt.scatter(obs[0], obs[1], color='k') + plt.plot(self.training_data[:,0], self.training_data[:,1], + label="Training Data") + plt.plot(state_hist[:,0], state_hist[:,1], + label="DMP Approximation") + plt.xlabel("Time [s]") + plt.ylabel("Position [m]") + plt.legend() + plt.show() return t, pos @@ -203,8 +233,8 @@ def show_DMP_purpose(self): linestyle='--') plt.plot(t_close, pos_close, label='Decreasing goal position', linestyle='--') - plt.xlabel("Time [s]") - plt.ylabel("Position [m]") + plt.xlabel("X Position") + plt.ylabel("Y Position") plt.legend() plt.show() @@ -212,8 +242,11 @@ def show_DMP_purpose(self): if __name__ == '__main__': period = np.pi / 2 t = np.arange(0, period, 0.01) - training_data = np.asarray([np.sin(t), np.cos(t)]).T + training_data = np.asarray([np.sin(t) + 0.02*np.random.rand(t.shape[0]), + np.cos(t) + 0.02*np.random.rand(t.shape[0]) ]).T DMP_controller = DMP(training_data, period) # DMP_controller.show_DMP_purpose() - DMP_controller.solve_trajectory([0,1], [1,0], 3) + + obs = np.asarray([[0.4,0.8], [1,0.5]]) + DMP_controller.solve_trajectory([0,1], [1,0], 3, obstacles=obs) From 29bdb1585d1aa8b6ee2bc4039b1017a157659c5c Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 18 Jul 2021 20:42:23 -0400 Subject: [PATCH 20/52] Potential field working much better but has issues with reaching goal state --- .../dynamic_movement_primitives.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index c649c58d7c..1dbf578c75 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -10,21 +10,20 @@ class DMP(object): - def __init__(self, training_data, data_period, K=156.25, B=25, dt=0.001, - timesteps=5000, repel_factor=1): + def __init__(self, training_data, data_period, K=156.25, B=25, + timesteps=500, repel_factor=1): """ Arguments: training_data - data in for [(x1,y1), (x2,y2), ...] data_period - amount of time training data covers K and B - spring and damper constants to define DMP behavior - dt - timestep between points in generated trajectories timesteps - number of points in generated trajectories repel_factor - controls how much path will avoid obstacles """ self.K = K # virtual spring constant self.B = B # virtual damper coefficient - self.dt = dt + self.dt = data_period / timesteps self.timesteps = timesteps self.weights = None # weights used to generate DMP trajectories @@ -146,6 +145,7 @@ def recreate_trajectory(self, init_state, goal_state, T): f = np.dot(Phi, self.weights[dim]) else: f = 0 + return np.arange(0, self.timesteps * self.dt, self.dt), positions # simulate dynamics qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 \ @@ -165,19 +165,20 @@ def recreate_trajectory(self, init_state, goal_state, T): def get_obstacle_force(self, state): - obstacle_force = np.asarray([0.0, 0.0]) + obstacle_force = np.zeros(len(self.obstacles[0])) for obs in self.obstacles: new_force = [] - dist = np.sum(np.sqrt((obs - state)**2)) + dist = np.sum(np.sqrt((state - obs)**2)) force = self.repel_factor * dist # TODO: all lists or all np arrays for inputs for dim in range(len(self.obstacles[0])): - new_force.append(force * (state[dim] - obs[dim]) / dist) + new_force.append(force * (state[dim] - obs[dim])**2) obstacle_force += np.array(new_force) + # print(obs, new_force, obstacle_force) return obstacle_force From 9e63e6d79086413a2c24c6e0a73f513edb1f8972 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Mon, 19 Jul 2021 23:12:13 -0400 Subject: [PATCH 21/52] Path ending to short not a result of obstacles, should be fix-able --- .../dynamic_movement_primitives.py | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 1dbf578c75..a5f6b97854 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -11,7 +11,7 @@ class DMP(object): def __init__(self, training_data, data_period, K=156.25, B=25, - timesteps=500, repel_factor=1): + timesteps=500, repel_factor=3): """ Arguments: training_data - data in for [(x1,y1), (x2,y2), ...] @@ -124,10 +124,11 @@ def recreate_trajectory(self, init_state, goal_state, T): positions = [] for k in range(self.timesteps): + # while(0.01 > self.dist_between(state, goal_state)): new_state = [] t = t + self.dt - obs_force = 0 + obs_force = np.zeros(self.weights.shape[0]) if self.obstacles is not None: obs_force = self.get_obstacle_force(state) @@ -163,6 +164,7 @@ def recreate_trajectory(self, init_state, goal_state, T): return np.arange(0, self.timesteps * self.dt, self.dt), positions + def get_obstacle_force(self, state): obstacle_force = np.zeros(len(self.obstacles[0])) @@ -193,14 +195,17 @@ def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None): if visualize: if self.training_data.shape[1] == 2: if self.obstacles is not None: - for obs in self.obstacles: - plt.scatter(obs[0], obs[1], color='k') + for i, obs in enumerate(self.obstacles): + if i == 0: + plt.scatter(obs[0], obs[1], color='k', label='Obstacle') + else: + plt.scatter(obs[0], obs[1], color='k') plt.plot(self.training_data[:,0], self.training_data[:,1], label="Training Data") plt.plot(state_hist[:,0], state_hist[:,1], label="DMP Approximation") - plt.xlabel("Time [s]") - plt.ylabel("Position [m]") + plt.xlabel("X Position") + plt.ylabel("Y Position") plt.legend() plt.show() @@ -249,5 +254,5 @@ def show_DMP_purpose(self): DMP_controller = DMP(training_data, period) # DMP_controller.show_DMP_purpose() - obs = np.asarray([[0.4,0.8], [1,0.5]]) - DMP_controller.solve_trajectory([0,1], [1,0], 3, obstacles=obs) + obs = np.asarray([[0.435,0.875], [1,0.5]]) + DMP_controller.solve_trajectory([0,1], [1,0], 3, obstacles=None) From d99c34a36bb57934192f5420f40d70a3a43e7d8c Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 21 Jul 2021 22:00:49 -0400 Subject: [PATCH 22/52] Mostly working! end won't go to goal --- .../dynamic_movement_primitives.py | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index a5f6b97854..518d914d59 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -6,6 +6,7 @@ """ import matplotlib.pyplot as plt import numpy as np +import copy class DMP(object): @@ -118,6 +119,8 @@ def recreate_trajectory(self, init_state, goal_state, T): state_double_dot = [0,0] t = 0 + + # for plotting self.train_t_vals = np.arange(0, T, self.timesteps) # TODO: is self.period variable used @@ -128,6 +131,7 @@ def recreate_trajectory(self, init_state, goal_state, T): new_state = [] t = t + self.dt + goal_force = self.goal_attraction(copy.deepcopy(state)) obs_force = np.zeros(self.weights.shape[0]) if self.obstacles is not None: obs_force = self.get_obstacle_force(state) @@ -150,7 +154,7 @@ def recreate_trajectory(self, init_state, goal_state, T): # simulate dynamics qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 \ - + obs_force[dim] + + obs_force[dim] + goal_force[dim] qd = qd + qdd * self.dt q = q + qd * self.dt @@ -177,13 +181,23 @@ def get_obstacle_force(self, state): # TODO: all lists or all np arrays for inputs for dim in range(len(self.obstacles[0])): - new_force.append(force * (state[dim] - obs[dim])**2) + new_force.append(force * (obs[dim] - state[dim])**2) obstacle_force += np.array(new_force) - # print(obs, new_force, obstacle_force) return obstacle_force + def goal_attraction(self, state): + + state = np.asarray(state) + + dx = state[0] - self.training_data[-1,0] + dy = state[1] - self.training_data[-1,1] + dist = np.sqrt(dx**2 + dy**2) + + return np.array([dx**2/dist, dy**2/dist])/2 + # return np.zeros(2) + def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None): self.obstacles = obstacles @@ -246,8 +260,8 @@ def show_DMP_purpose(self): if __name__ == '__main__': - period = np.pi / 2 - t = np.arange(0, period, 0.01) + period = 6*np.pi + t = np.arange(0, np.pi/2, 0.01) training_data = np.asarray([np.sin(t) + 0.02*np.random.rand(t.shape[0]), np.cos(t) + 0.02*np.random.rand(t.shape[0]) ]).T @@ -255,4 +269,6 @@ def show_DMP_purpose(self): # DMP_controller.show_DMP_purpose() obs = np.asarray([[0.435,0.875], [1,0.5]]) - DMP_controller.solve_trajectory([0,1], [1,0], 3, obstacles=None) + # obs = np.asarray([[0.435,0.875]]) + # obs = np.asarray([[1,0.5]]) + DMP_controller.solve_trajectory([0,1], [1,0], 3, obstacles=obs) From 910498bd2302f6b2ce54b3c57a1ed905cc3436f5 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 21 Jul 2021 23:59:57 -0400 Subject: [PATCH 23/52] split DMP and path following --- .../dynamic_movement_primitives.py | 114 ++++++++++-------- 1 file changed, 67 insertions(+), 47 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 518d914d59..74095d7032 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -12,7 +12,7 @@ class DMP(object): def __init__(self, training_data, data_period, K=156.25, B=25, - timesteps=500, repel_factor=3): + timesteps=500, repel_factor=2): """ Arguments: training_data - data in for [(x1,y1), (x2,y2), ...] @@ -28,15 +28,17 @@ def __init__(self, training_data, data_period, K=156.25, B=25, self.timesteps = timesteps self.weights = None # weights used to generate DMP trajectories + self.obstacles = None self.repel_factor = repel_factor + self.DMP_path_attraction = 10 # self.T_orig = data_period # self.training_data = training_data self.find_basis_functions_weights(training_data, data_period) - def find_basis_functions_weights(self, training_data, data_period, num_weights=5): + def find_basis_functions_weights(self, training_data, data_period, num_weights=10): """ Arguments: data [(steps x spacial dim) np array] - data to replicate with DMP @@ -100,11 +102,12 @@ def find_basis_functions_weights(self, training_data, data_period, num_weights=5 return self.weights # TODO: neccesary? - def recreate_trajectory(self, init_state, goal_state, T): + def recreate_trajectory(self, init_state, goal_state, T, path): """ init_state - initial state/position goal_state - goal state/position T - amount of time to travek q0 -> g + path - TODO """ nrBasis = len(self.weights[0]) # number of gaussian basis functions @@ -114,56 +117,61 @@ def recreate_trajectory(self, init_state, goal_state, T): H = (0.65*(1./(nrBasis-1))**2) # initialize virtual system - state = init_state - state_dot = [0,0] - state_double_dot = [0,0] t = 0 - - # for plotting self.train_t_vals = np.arange(0, T, self.timesteps) # TODO: is self.period variable used + if not isinstance(init_state, np.ndarray): + init_state = np.asarray(init_state) + if not isinstance(goal_state, np.ndarray): + goal_state = np.asarray(goal_state) + + q = init_state + dimensions = self.weights.shape[0] + qd = np.zeros(dimensions) + qdd = np.zeros(dimensions) + positions = [] for k in range(self.timesteps): - # while(0.01 > self.dist_between(state, goal_state)): new_state = [] t = t + self.dt - goal_force = self.goal_attraction(copy.deepcopy(state)) - obs_force = np.zeros(self.weights.shape[0]) - if self.obstacles is not None: - obs_force = self.get_obstacle_force(state) + if path is not None: - for dim in range(self.weights.shape[0]): + obs_force = np.zeros(dimensions) + if self.obstacles is not None: + obs_force = self.get_obstacle_force(q) + qdd = self.K*(goal_state - q)/T**2/10 - self.B*qd/T + self.goal_attraction(q, path[k]) + obs_force - q0 = init_state[dim] - q = state[dim] - qd = state_dot[dim] - qdd = state_double_dot[dim] - g = goal_state[dim] + if 0.01 > self.dist_between(q, goal_state): + break - if t <= T: - Phi = [np.exp(-0.5 * ((t / T) - c)**2 / H) for c in C] - Phi = Phi / np.sum(Phi) - f = np.dot(Phi, self.weights[dim]) - else: - f = 0 - return np.arange(0, self.timesteps * self.dt, self.dt), positions + else: + + qdd = np.zeros(dimensions) + + for dim in range(dimensions): - # simulate dynamics - qdd = self.K*(g-q)/T**2 - self.B*qd/T + (g-q0)*f/T**2 \ - + obs_force[dim] + goal_force[dim] - qd = qd + qdd * self.dt - q = q + qd * self.dt + if t <= T: + Phi = [np.exp(-0.5 * ((t / T) - c)**2 / H) for c in C] + Phi = Phi / np.sum(Phi) + f = np.dot(Phi, self.weights[dim]) + else: + f = 0 - state_dot[dim] = qd - state_double_dot[dim] = qdd + # simulate dynamics + # print(self.K*(goal_state[dim] - q[dim])/T**2)# - self.B*qd[dim]/T + (goal_state[dim] - init_state[dim])*f/T**2) + qdd[dim] = self.K*(goal_state[dim] - q[dim])/T**2 - self.B*qd[dim]/T + (goal_state[dim] - init_state[dim])*f/T**2 - new_state.append(q) + qd = qd + qdd * self.dt + q = q + qd * self.dt + + # TODO: get rid of this + if not isinstance(q, list): + new_state = q.tolist() - state = new_state positions.append(new_state) return np.arange(0, self.timesteps * self.dt, self.dt), positions @@ -176,7 +184,7 @@ def get_obstacle_force(self, state): for obs in self.obstacles: new_force = [] - dist = np.sum(np.sqrt((state - obs)**2)) + dist = -np.sum(np.sqrt((state - obs)**2)) force = self.repel_factor * dist # TODO: all lists or all np arrays for inputs @@ -185,26 +193,35 @@ def get_obstacle_force(self, state): obstacle_force += np.array(new_force) - return obstacle_force + return obstacle_force*10 - def goal_attraction(self, state): + def goal_attraction(self, state, path_point): state = np.asarray(state) - dx = state[0] - self.training_data[-1,0] - dy = state[1] - self.training_data[-1,1] - dist = np.sqrt(dx**2 + dy**2) + # print(state, path_point) + dx = state[0] - path_point[0] + dy = state[1] - path_point[1] + dist = -np.sqrt(dx**2 + dy**2) + + att_force = np.array([dx/dist, dy/dist])*self.DMP_path_attraction + return att_force - return np.array([dx**2/dist, dy**2/dist])/2 - # return np.zeros(2) + @staticmethod + def dist_between(p1, p2): + return np.linalg.norm(p1 - p2) def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None): - self.obstacles = obstacles - t, pos = self.recreate_trajectory(q0, g, T) + _, path = self.recreate_trajectory(q0, g, T, None) + self.obstacles = obstacles + t, pos = self.recreate_trajectory(q0, g, T, path) state_hist = np.asarray(pos) + path = np.asarray(path) + + # state_hist = np.asarray(pos) if visualize: if self.training_data.shape[1] == 2: @@ -216,8 +233,11 @@ def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None): plt.scatter(obs[0], obs[1], color='k') plt.plot(self.training_data[:,0], self.training_data[:,1], label="Training Data") - plt.plot(state_hist[:,0], state_hist[:,1], + plt.plot(path[:,0], path[:,1], label="DMP Approximation") + plt.plot(state_hist[:,0], state_hist[:,1], + label="path") + plt.xlabel("X Position") plt.ylabel("Y Position") plt.legend() @@ -260,7 +280,7 @@ def show_DMP_purpose(self): if __name__ == '__main__': - period = 6*np.pi + period = 2*np.pi t = np.arange(0, np.pi/2, 0.01) training_data = np.asarray([np.sin(t) + 0.02*np.random.rand(t.shape[0]), np.cos(t) + 0.02*np.random.rand(t.shape[0]) ]).T From 85d9528d4f436e498c77387030de3c45acff41b5 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 22 Jul 2021 22:02:04 -0400 Subject: [PATCH 24/52] pretty close --- .../dynamic_movement_primitives.py | 33 +++++++++++-------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 74095d7032..6b8569e5c2 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -12,7 +12,7 @@ class DMP(object): def __init__(self, training_data, data_period, K=156.25, B=25, - timesteps=500, repel_factor=2): + timesteps=500, repel_factor=5): """ Arguments: training_data - data in for [(x1,y1), (x2,y2), ...] @@ -143,11 +143,21 @@ def recreate_trajectory(self, init_state, goal_state, T, path): obs_force = np.zeros(dimensions) if self.obstacles is not None: obs_force = self.get_obstacle_force(q) - qdd = self.K*(goal_state - q)/T**2/10 - self.B*qd/T + self.goal_attraction(q, path[k]) + obs_force + goal_dist = self.dist_between(q, goal_state) - if 0.01 > self.dist_between(q, goal_state): + if 0.01 > goal_dist: break + goal_force = 0 + # if np.linalg.norm(obs_force) > 0: + # if(self.dist_between(goal_state, q) * 5 < self.dist_between(init_state, goal_state)): + # goal_force = self.K*(goal_state - q)/5 + + print(k, goal_force, obs_force) + + qdd = 4*self.K*(path[k] - q)/T**2 - 0.1*self.B*qd/T + goal_force -0.002*obs_force + # - obs_force + else: qdd = np.zeros(dimensions) @@ -162,7 +172,6 @@ def recreate_trajectory(self, init_state, goal_state, T, path): f = 0 # simulate dynamics - # print(self.K*(goal_state[dim] - q[dim])/T**2)# - self.B*qd[dim]/T + (goal_state[dim] - init_state[dim])*f/T**2) qdd[dim] = self.K*(goal_state[dim] - q[dim])/T**2 - self.B*qd[dim]/T + (goal_state[dim] - init_state[dim])*f/T**2 qd = qd + qdd * self.dt @@ -184,22 +193,21 @@ def get_obstacle_force(self, state): for obs in self.obstacles: new_force = [] - dist = -np.sum(np.sqrt((state - obs)**2)) + dist = np.sum(np.sqrt((state - obs)**2)) force = self.repel_factor * dist # TODO: all lists or all np arrays for inputs for dim in range(len(self.obstacles[0])): - new_force.append(force * (obs[dim] - state[dim])**2) + new_force.append(force / (obs[dim] - state[dim])) - obstacle_force += np.array(new_force) + obstacle_force += np.array(new_force) - return obstacle_force*10 + return obstacle_force def goal_attraction(self, state, path_point): state = np.asarray(state) - # print(state, path_point) dx = state[0] - path_point[0] dy = state[1] - path_point[1] dist = -np.sqrt(dx**2 + dy**2) @@ -213,7 +221,6 @@ def dist_between(p1, p2): def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None): - _, path = self.recreate_trajectory(q0, g, T, None) self.obstacles = obstacles @@ -288,7 +295,7 @@ def show_DMP_purpose(self): DMP_controller = DMP(training_data, period) # DMP_controller.show_DMP_purpose() - obs = np.asarray([[0.435,0.875], [1,0.5]]) - # obs = np.asarray([[0.435,0.875]]) - # obs = np.asarray([[1,0.5]]) + # obs = np.asarray([[0.435,0.8], [0.87,0.5]]) + # obs = np.asarray([[0.435,0.8]]) + obs = np.asarray([[0.83,0.7]]) DMP_controller.solve_trajectory([0,1], [1,0], 3, obstacles=obs) From 6afbcf7722eb89eb06ad465674054d4d29b160a3 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 25 Jul 2021 16:46:15 -0400 Subject: [PATCH 25/52] Okay this is working pretty well --- .../dynamic_movement_primitives.py | 25 ++++++++----------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 6b8569e5c2..c94de34145 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -12,7 +12,7 @@ class DMP(object): def __init__(self, training_data, data_period, K=156.25, B=25, - timesteps=500, repel_factor=5): + timesteps=2500, repel_factor=0.1): """ Arguments: training_data - data in for [(x1,y1), (x2,y2), ...] @@ -145,18 +145,14 @@ def recreate_trajectory(self, init_state, goal_state, T, path): obs_force = self.get_obstacle_force(q) goal_dist = self.dist_between(q, goal_state) - if 0.01 > goal_dist: + if 0.02 > goal_dist: break goal_force = 0 - # if np.linalg.norm(obs_force) > 0: - # if(self.dist_between(goal_state, q) * 5 < self.dist_between(init_state, goal_state)): - # goal_force = self.K*(goal_state - q)/5 - print(k, goal_force, obs_force) - - qdd = 4*self.K*(path[k] - q)/T**2 - 0.1*self.B*qd/T + goal_force -0.002*obs_force - # - obs_force + qdd = self.B*qd/T + # - obs_force + q = path[k] - obs_force else: @@ -194,13 +190,12 @@ def get_obstacle_force(self, state): new_force = [] dist = np.sum(np.sqrt((state - obs)**2)) - force = self.repel_factor * dist + + force = self.repel_factor / dist # TODO: all lists or all np arrays for inputs for dim in range(len(self.obstacles[0])): - new_force.append(force / (obs[dim] - state[dim])) - - obstacle_force += np.array(new_force) + obstacle_force[dim] = (force * (obs[dim] - state[dim])) return obstacle_force @@ -295,7 +290,7 @@ def show_DMP_purpose(self): DMP_controller = DMP(training_data, period) # DMP_controller.show_DMP_purpose() - # obs = np.asarray([[0.435,0.8], [0.87,0.5]]) + obs = np.asarray([[0.435,0.8], [0.87,0.5]]) # obs = np.asarray([[0.435,0.8]]) - obs = np.asarray([[0.83,0.7]]) + # obs = np.asarray([[0.7,0.7]]) DMP_controller.solve_trajectory([0,1], [1,0], 3, obstacles=obs) From 5f0aeedcf94cb9e1f26765e7fc05ff807e9978c6 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 25 Jul 2021 21:42:52 -0400 Subject: [PATCH 26/52] looks.. okay. was using the wrong vector before --- .../dynamic_movement_primitives.py | 30 +++++++++++++++---- 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index c94de34145..fd27e7e7e4 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -12,7 +12,7 @@ class DMP(object): def __init__(self, training_data, data_period, K=156.25, B=25, - timesteps=2500, repel_factor=0.1): + timesteps=2500, repel_factor=0.001): """ Arguments: training_data - data in for [(x1,y1), (x2,y2), ...] @@ -141,8 +141,12 @@ def recreate_trajectory(self, init_state, goal_state, T, path): if path is not None: obs_force = np.zeros(dimensions) + path_norm_vec = np.zeros(2) + if k+1 < self.timesteps: + path_norm_vec = self.get_vec_normal_to_path(path[k], path[k+1]) + if self.obstacles is not None: - obs_force = self.get_obstacle_force(q) + obs_force = self.get_obstacle_force(path[k], path_norm_vec) goal_dist = self.dist_between(q, goal_state) if 0.02 > goal_dist: @@ -179,10 +183,21 @@ def recreate_trajectory(self, init_state, goal_state, T, path): positions.append(new_state) + pos_arr = np.asarray(positions) + dists = [] + for i in range(1,pos_arr.shape[0]): + dists.append(self.dist_between(pos_arr[i-1], pos_arr[i])) + dists.sort() + + return np.arange(0, self.timesteps * self.dt, self.dt), positions + def get_vec_normal_to_path(self, last_state, next_state): + diffs = np.array([next_state[0] - last_state[0], next_state[1] - last_state[1]]) + normal_vec = np.array([diffs[0], -1 / diffs[1]]) + return normal_vec / np.linalg.norm(normal_vec) - def get_obstacle_force(self, state): + def get_obstacle_force(self, state, normal_vec): obstacle_force = np.zeros(len(self.obstacles[0])) @@ -191,11 +206,14 @@ def get_obstacle_force(self, state): dist = np.sum(np.sqrt((state - obs)**2)) - force = self.repel_factor / dist + force = self.repel_factor / dist**2 + + + obstacle_force = (force * (obs - state)) @ normal_vec # TODO: all lists or all np arrays for inputs - for dim in range(len(self.obstacles[0])): - obstacle_force[dim] = (force * (obs[dim] - state[dim])) + # for dim in range(len(self.obstacles[0])): + # obstacle_force[dim] = (force * (obs[dim] - state[dim])) return obstacle_force From 1439b4efe9fc330bf3caa3451db7e6989dc630e0 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sun, 25 Jul 2021 22:22:43 -0400 Subject: [PATCH 27/52] a plan to fix this mess --- .../dynamic_movement_primitives.py | 31 +++++++++++++++++-- 1 file changed, 28 insertions(+), 3 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index fd27e7e7e4..c134e91e50 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -12,7 +12,7 @@ class DMP(object): def __init__(self, training_data, data_period, K=156.25, B=25, - timesteps=2500, repel_factor=0.001): + timesteps=500, repel_factor=0.001): """ Arguments: training_data - data in for [(x1,y1), (x2,y2), ...] @@ -145,6 +145,7 @@ def recreate_trajectory(self, init_state, goal_state, T, path): if k+1 < self.timesteps: path_norm_vec = self.get_vec_normal_to_path(path[k], path[k+1]) + print(path_norm_vec, q) if self.obstacles is not None: obs_force = self.get_obstacle_force(path[k], path_norm_vec) goal_dist = self.dist_between(q, goal_state) @@ -201,6 +202,7 @@ def get_obstacle_force(self, state, normal_vec): obstacle_force = np.zeros(len(self.obstacles[0])) + print("shooby dooby!") for obs in self.obstacles: new_force = [] @@ -209,8 +211,10 @@ def get_obstacle_force(self, state, normal_vec): force = self.repel_factor / dist**2 + print("\t", obs, dist, force) - obstacle_force = (force * (obs - state)) @ normal_vec + + obstacle_force += (force * (obs - state)) @ normal_vec # TODO: all lists or all np arrays for inputs # for dim in range(len(self.obstacles[0])): # obstacle_force[dim] = (force * (obs[dim] - state[dim])) @@ -308,7 +312,28 @@ def show_DMP_purpose(self): DMP_controller = DMP(training_data, period) # DMP_controller.show_DMP_purpose() - obs = np.asarray([[0.435,0.8], [0.87,0.5]]) + obs = np.asarray([[0.435,0.9], [0.87,0.5]]) # obs = np.asarray([[0.435,0.8]]) # obs = np.asarray([[0.7,0.7]]) DMP_controller.solve_trajectory([0,1], [1,0], 3, obstacles=obs) + +""" +okay here's how to handle the obstacles: +- draw circle around each obstacle point +- dubins curve from two points before first intersection to furthest point on circle + (create angle with obstacle point as center & the two points [next to intersection points && not inside circle] + and draw bisector of circle_radius, add point with heading tangent to circ there) + -> probably want points to be slightly further than radius length to give dubins some slack to work with +- dubins curve from the two circle tangent points from bisector to the two points right after second intersection +- to handle close obstacles: + for multiple obs in obstacles: + if dist_nearest_obstacle(obs) < turning_radius: + -average two points + - draw larger circle around average (~1.5x max(turning_radius, dist(obs, nearest_obs))) + - remove two points from obs, add larger circ (or maybe just have a separate storage for these) + - next point is skipped in loop + - keep doing above loop until it loops through all points without making a change + + + +""" From 2defd14004e9114020e72bcdb5cbb20c5ec26248 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Mon, 26 Jul 2021 21:46:59 -0400 Subject: [PATCH 28/52] Okay seriously going to pivot to the dubins approach im done with potential field lol --- .../dynamic_movement_primitives.py | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index c134e91e50..312e41a8cc 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -31,6 +31,7 @@ def __init__(self, training_data, data_period, K=156.25, B=25, self.obstacles = None self.repel_factor = repel_factor + self.avoidance_distance = 0.1 self.DMP_path_attraction = 10 # self.T_orig = data_period @@ -145,9 +146,9 @@ def recreate_trajectory(self, init_state, goal_state, T, path): if k+1 < self.timesteps: path_norm_vec = self.get_vec_normal_to_path(path[k], path[k+1]) - print(path_norm_vec, q) + print("path norm vec q", path_norm_vec, q) if self.obstacles is not None: - obs_force = self.get_obstacle_force(path[k], path_norm_vec) + obs_force = self.get_obstacle_force((q+path[k])/2, path_norm_vec) goal_dist = self.dist_between(q, goal_state) if 0.02 > goal_dist: @@ -198,11 +199,13 @@ def get_vec_normal_to_path(self, last_state, next_state): normal_vec = np.array([diffs[0], -1 / diffs[1]]) return normal_vec / np.linalg.norm(normal_vec) + def account_for_obstacles(self, path): + raise notimplementederror + def get_obstacle_force(self, state, normal_vec): obstacle_force = np.zeros(len(self.obstacles[0])) - print("shooby dooby!") for obs in self.obstacles: new_force = [] @@ -211,10 +214,10 @@ def get_obstacle_force(self, state, normal_vec): force = self.repel_factor / dist**2 - print("\t", obs, dist, force) + print("\t obs dist force the quantity", obs, dist, force, ((obs - state) @ normal_vec)) - obstacle_force += (force * (obs - state)) @ normal_vec + obstacle_force += force * ((obs - state) @ normal_vec) / np.linalg.norm(obs - state) # TODO: all lists or all np arrays for inputs # for dim in range(len(self.obstacles[0])): # obstacle_force[dim] = (force * (obs[dim] - state[dim])) @@ -305,14 +308,17 @@ def show_DMP_purpose(self): if __name__ == '__main__': period = 2*np.pi - t = np.arange(0, np.pi/2, 0.01) - training_data = np.asarray([np.sin(t) + 0.02*np.random.rand(t.shape[0]), - np.cos(t) + 0.02*np.random.rand(t.shape[0]) ]).T + # t = np.arange(0, np.pi/2, 0.01) + # training_data = np.asarray([np.sin(t) + 0.02*np.random.rand(t.shape[0]), + # np.cos(t) + 0.02*np.random.rand(t.shape[0]) ]).T + t = np.arange(0,1,0.01) + training_data = np.asarray([t, np.flip(t,0)]).T DMP_controller = DMP(training_data, period) # DMP_controller.show_DMP_purpose() - obs = np.asarray([[0.435,0.9], [0.87,0.5]]) + obs = np.asarray([[0.3, 0.6], [0.7,0.25]]) + # obs = np.asarray([[0.435,0.9], [0.87,0.5]]) # obs = np.asarray([[0.435,0.8]]) # obs = np.asarray([[0.7,0.7]]) DMP_controller.solve_trajectory([0,1], [1,0], 3, obstacles=obs) From db7962633407ad65f648f9f559140da293359c95 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 29 Jul 2021 21:45:07 -0400 Subject: [PATCH 29/52] Finished obstacle circle handling (and merging circles that are closer than their radii) --- .../dynamic_movement_primitives.py | 64 ++++++++++++++++++- 1 file changed, 61 insertions(+), 3 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 312e41a8cc..214ba61d5f 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -7,6 +7,23 @@ import matplotlib.pyplot as plt import numpy as np import copy +import math as m + +class ObstacleCircle(object): + def __init__(self, origin, radius): + self.x = origin[0] + self.y = origin[1] + self.radius = radius + + def distance_to(self, item): + if isinstance(item, ObstacleCircle): + return m.sqrt((self.x - item.x)**2 + (self.y - item.y)**2) + else: + # assume point + return m.sqrt((self.x - item[0])**2 + (self.y - item[1])**2) + + def __repr__(self): + return "Origin: (" + str(self.x) + ", " + str(self.y) + ") ~ r" + str(self.radius) class DMP(object): @@ -141,12 +158,14 @@ def recreate_trajectory(self, init_state, goal_state, T, path): if path is not None: + self.account_for_obstacles(path) + obs_force = np.zeros(dimensions) path_norm_vec = np.zeros(2) if k+1 < self.timesteps: path_norm_vec = self.get_vec_normal_to_path(path[k], path[k+1]) - print("path norm vec q", path_norm_vec, q) + # print("path norm vec q", path_norm_vec, q) if self.obstacles is not None: obs_force = self.get_obstacle_force((q+path[k])/2, path_norm_vec) goal_dist = self.dist_between(q, goal_state) @@ -200,7 +219,46 @@ def get_vec_normal_to_path(self, last_state, next_state): return normal_vec / np.linalg.norm(normal_vec) def account_for_obstacles(self, path): - raise notimplementederror + obstacle_circles = [] + for obs in self.obstacles: + obstacle_circles.append(ObstacleCircle(obs, self.avoidance_distance)) + + # combine circles that overlap + i = 0 + while(i < len(obstacle_circles)-1): + j = i+1 + while(j < len(obstacle_circles)): + circ_dist = obstacle_circles[i].distance_to(obstacle_circles[j]) + if circ_dist < max(obstacle_circles[i].radius, obstacle_circles[j].radius): + + circ2 = obstacle_circles.pop(j) + circ1 = obstacle_circles.pop(i) + new_circ_x = (circ1.x + circ2.x)/2 + new_circ_y = (circ1.y + circ2.y)/2 + new_circ_rad = max(circ1.radius, circ2.radius) + circ_dist + + obstacle_circles.append(ObstacleCircle([new_circ_x, new_circ_y], new_circ_rad)) + i = -1 # gets set to 0 by i++ below + break + + else: + j += 1 + i += 1 + + for circ in obstacle_circles: + path = self.add_circle_to_path(path, circ) + + def add_circle_to_path(self, path, circle): + """ + TODO + - find intersections between obstacle and path (if any, if none return path) + - draw perpendicular line to path through circle center + - of 2 intersections perp line makes with circ, use one that is closer to path + - draw dubins from before first int to that point + - draw dubins from that point to after second int + """ + + return path def get_obstacle_force(self, state, normal_vec): @@ -214,7 +272,7 @@ def get_obstacle_force(self, state, normal_vec): force = self.repel_factor / dist**2 - print("\t obs dist force the quantity", obs, dist, force, ((obs - state) @ normal_vec)) + # print("\t obs dist force the quantity", obs, dist, force, ((obs - state) @ normal_vec)) obstacle_force += force * ((obs - state) @ normal_vec) / np.linalg.norm(obs - state) From 28fbc08a76e2b1bf4495b765e0fc8159cbc496ef Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 4 Aug 2021 22:12:28 -0400 Subject: [PATCH 30/52] Finished circle event finder function --- .../dynamic_movement_primitives.py | 39 ++++++++++++++++--- 1 file changed, 33 insertions(+), 6 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 214ba61d5f..5ff79b4c47 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -19,12 +19,21 @@ def distance_to(self, item): if isinstance(item, ObstacleCircle): return m.sqrt((self.x - item.x)**2 + (self.y - item.y)**2) else: - # assume point + # assume point that can be indexed return m.sqrt((self.x - item[0])**2 + (self.y - item[1])**2) def __repr__(self): return "Origin: (" + str(self.x) + ", " + str(self.y) + ") ~ r" + str(self.radius) +class CircleEvent(ObstacleCircle): + + def __init__(self, circle, start_pt): + self.circle = circle + self.start_pt = start_pt + self.end_pt = None + + def __repr__(self): + return "Origin: (" + str(self.x) + ", " + str(self.y) + ") ~ r" + str(self.radius) class DMP(object): @@ -245,20 +254,38 @@ def account_for_obstacles(self, path): j += 1 i += 1 - for circ in obstacle_circles: - path = self.add_circle_to_path(path, circ) + circle_events = self.get_circle_events(path, obstacle_circles) - def add_circle_to_path(self, path, circle): """ TODO - - find intersections between obstacle and path (if any, if none return path) + - draw perpendicular line to path through circle center - of 2 intersections perp line makes with circ, use one that is closer to path - draw dubins from before first int to that point - draw dubins from that point to after second int """ - return path + def get_circle_events(self, path, circles): + + circle_flags = np.zeros(len(circles)) + + circle_events = [] + for i in range(len(circles)): + circle_events.append(None) + + for path_pt in path: + for i, circ in enumerate(circles): + dist = circ.distance_to(path_pt) + if dist < self.avoidance_distance and not circle_flags[i]: + # circle in range + circle_flags[i] = 1 + circle_events[i] = (CircleEvent(circ, path_pt)) + elif dist > self.avoidance_distance and circle_flags[i]: + # circle out of range + circle_flags[i] = 0 + circle_events[i].end_pt = path_pt + + return circle_events def get_obstacle_force(self, state, normal_vec): From d3b1694516427a1a8a71635167475ef3dad98150 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Tue, 10 Aug 2021 22:42:34 -0400 Subject: [PATCH 31/52] Some progress in preparing for dubins curves --- .../dynamic_movement_primitives.py | 42 ++++++++++++++----- 1 file changed, 32 insertions(+), 10 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 5ff79b4c47..8408a3628b 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -7,7 +7,7 @@ import matplotlib.pyplot as plt import numpy as np import copy -import math as m +import math class ObstacleCircle(object): def __init__(self, origin, radius): @@ -17,20 +17,20 @@ def __init__(self, origin, radius): def distance_to(self, item): if isinstance(item, ObstacleCircle): - return m.sqrt((self.x - item.x)**2 + (self.y - item.y)**2) + return math.sqrt((self.x - item.x)**2 + (self.y - item.y)**2) else: # assume point that can be indexed - return m.sqrt((self.x - item[0])**2 + (self.y - item[1])**2) + return math.sqrt((self.x - item[0])**2 + (self.y - item[1])**2) def __repr__(self): return "Origin: (" + str(self.x) + ", " + str(self.y) + ") ~ r" + str(self.radius) class CircleEvent(ObstacleCircle): - def __init__(self, circle, start_pt): + def __init__(self, circle, start_pt_idx): self.circle = circle - self.start_pt = start_pt - self.end_pt = None + self.start_pt_idx = start_pt_idx + self.end_pt_idx = None def __repr__(self): return "Origin: (" + str(self.x) + ", " + str(self.y) + ") ~ r" + str(self.radius) @@ -219,7 +219,6 @@ def recreate_trajectory(self, init_state, goal_state, T, path): dists.append(self.dist_between(pos_arr[i-1], pos_arr[i])) dists.sort() - return np.arange(0, self.timesteps * self.dt, self.dt), positions def get_vec_normal_to_path(self, last_state, next_state): @@ -256,6 +255,29 @@ def account_for_obstacles(self, path): circle_events = self.get_circle_events(path, obstacle_circles) + for event in circle_events: + start_pt = path[event.start_pt_idx] + end_pt = path[event.end_pt_idx] + + circ_center = event.circle + + start_ang = math.atan2(start_pt[1] - circ_center[1], + start_pt[0] - circ_center[0]) + + end_ang = math.atan2(end_pt[1] - circ_center[1], + end_pt[0] - circ_center[0]) + + count_up = False + + if(start_ang > end_ang): + if(start_ang - end_ang < math.pi): + count_up = True + else: + if(end_ang - start_ang < math.pi): + count_up = True + + # just find point that is halfway through arc and use that + """ TODO @@ -273,17 +295,17 @@ def get_circle_events(self, path, circles): for i in range(len(circles)): circle_events.append(None) - for path_pt in path: + for path_dex, path_pt in enumerate(path): for i, circ in enumerate(circles): dist = circ.distance_to(path_pt) if dist < self.avoidance_distance and not circle_flags[i]: # circle in range circle_flags[i] = 1 - circle_events[i] = (CircleEvent(circ, path_pt)) + circle_events[i] = (CircleEvent(circ, path_dex)) elif dist > self.avoidance_distance and circle_flags[i]: # circle out of range circle_flags[i] = 0 - circle_events[i].end_pt = path_pt + circle_events[i].end_pt_idx = path_dex return circle_events From b00812cfe8880aec5e3e4d68bd38890c029e96ce Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 11 Aug 2021 21:40:21 -0400 Subject: [PATCH 32/52] Finished angle finding algo, need to test --- .../dynamic_movement_primitives.py | 32 +++++++++++++++---- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 8408a3628b..1581804dd6 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -267,14 +267,32 @@ def account_for_obstacles(self, path): end_ang = math.atan2(end_pt[1] - circ_center[1], end_pt[0] - circ_center[0]) - count_up = False + # TODO: put in its own function + start_center_slope = (start_pt[1] - circ_center[1]) / (start_pt[0] - circ_center[0]) + tangent_at_start = -1 /start_center_slope + tangent_vec = [1, tangent_at_start] / math.sqrt(1 + tangent_at_start**2) - if(start_ang > end_ang): - if(start_ang - end_ang < math.pi): - count_up = True - else: - if(end_ang - start_ang < math.pi): - count_up = True + chord_vec = np.asarray([end_pt[0] - start_pt[0], end_pt[1] - start_pt[1]]) + chord_vec = chord_vec / np.linalg.norm(chord_vec) + + angle_from_tangent = chord_vec - tangent_vec + + while angle_from_tangent > math.pi: + angle_from_tangent -= math.pi + + while angle_from_tangent < -math.pi: + angle_from_tangent += math.pi + + arc_angle = 2 * angle_from_tangent + end_angle = start_ang + arc_angle + + + # if(start_ang > end_ang): + # if(start_ang - end_ang < math.pi): + # count_up = True + # else: + # if(end_ang - start_ang < math.pi): + # count_up = True # just find point that is halfway through arc and use that From 6ab4a42f0a2bdc488126205b3ff2413d659f0d40 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Tue, 14 Sep 2021 16:25:33 -0400 Subject: [PATCH 33/52] Okay getting back to this, going to ignore the navigation and just focus on path generation since that's what DMP is for --- .../dynamic_movement_primitives.py | 328 +++--------------- 1 file changed, 55 insertions(+), 273 deletions(-) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py index 1581804dd6..5ad62520b3 100644 --- a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py +++ b/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py @@ -9,32 +9,6 @@ import copy import math -class ObstacleCircle(object): - def __init__(self, origin, radius): - self.x = origin[0] - self.y = origin[1] - self.radius = radius - - def distance_to(self, item): - if isinstance(item, ObstacleCircle): - return math.sqrt((self.x - item.x)**2 + (self.y - item.y)**2) - else: - # assume point that can be indexed - return math.sqrt((self.x - item[0])**2 + (self.y - item[1])**2) - - def __repr__(self): - return "Origin: (" + str(self.x) + ", " + str(self.y) + ") ~ r" + str(self.radius) - -class CircleEvent(ObstacleCircle): - - def __init__(self, circle, start_pt_idx): - self.circle = circle - self.start_pt_idx = start_pt_idx - self.end_pt_idx = None - - def __repr__(self): - return "Origin: (" + str(self.x) + ", " + str(self.y) + ") ~ r" + str(self.radius) - class DMP(object): def __init__(self, training_data, data_period, K=156.25, B=25, @@ -60,7 +34,7 @@ def __init__(self, training_data, data_period, K=156.25, B=25, self.avoidance_distance = 0.1 self.DMP_path_attraction = 10 - # self.T_orig = data_period + self.T_orig = data_period # self.training_data = training_data self.find_basis_functions_weights(training_data, data_period) @@ -129,7 +103,8 @@ def find_basis_functions_weights(self, training_data, data_period, num_weights=1 return self.weights # TODO: neccesary? - def recreate_trajectory(self, init_state, goal_state, T, path): + + def recreate_trajectory(self, init_state, goal_state, T): """ init_state - initial state/position goal_state - goal state/position @@ -165,44 +140,19 @@ def recreate_trajectory(self, init_state, goal_state, T, path): new_state = [] t = t + self.dt - if path is not None: - - self.account_for_obstacles(path) - - obs_force = np.zeros(dimensions) - path_norm_vec = np.zeros(2) - if k+1 < self.timesteps: - path_norm_vec = self.get_vec_normal_to_path(path[k], path[k+1]) + qdd = np.zeros(dimensions) - # print("path norm vec q", path_norm_vec, q) - if self.obstacles is not None: - obs_force = self.get_obstacle_force((q+path[k])/2, path_norm_vec) - goal_dist = self.dist_between(q, goal_state) - - if 0.02 > goal_dist: - break - - goal_force = 0 + for dim in range(dimensions): - qdd = self.B*qd/T - # - obs_force - q = path[k] - obs_force - - else: - - qdd = np.zeros(dimensions) - - for dim in range(dimensions): - - if t <= T: - Phi = [np.exp(-0.5 * ((t / T) - c)**2 / H) for c in C] - Phi = Phi / np.sum(Phi) - f = np.dot(Phi, self.weights[dim]) - else: - f = 0 + if t <= T: + Phi = [np.exp(-0.5 * ((t / T) - c)**2 / H) for c in C] + Phi = Phi / np.sum(Phi) + f = np.dot(Phi, self.weights[dim]) + else: + f = 0 - # simulate dynamics - qdd[dim] = self.K*(goal_state[dim] - q[dim])/T**2 - self.B*qd[dim]/T + (goal_state[dim] - init_state[dim])*f/T**2 + # simulate dynamics + qdd[dim] = self.K*(goal_state[dim] - q[dim])/T**2 - self.B*qd[dim]/T + (goal_state[dim] - init_state[dim])*f/T**2 qd = qd + qdd * self.dt q = q + qd * self.dt @@ -213,168 +163,17 @@ def recreate_trajectory(self, init_state, goal_state, T, path): positions.append(new_state) - pos_arr = np.asarray(positions) - dists = [] - for i in range(1,pos_arr.shape[0]): - dists.append(self.dist_between(pos_arr[i-1], pos_arr[i])) - dists.sort() - return np.arange(0, self.timesteps * self.dt, self.dt), positions - def get_vec_normal_to_path(self, last_state, next_state): - diffs = np.array([next_state[0] - last_state[0], next_state[1] - last_state[1]]) - normal_vec = np.array([diffs[0], -1 / diffs[1]]) - return normal_vec / np.linalg.norm(normal_vec) - - def account_for_obstacles(self, path): - obstacle_circles = [] - for obs in self.obstacles: - obstacle_circles.append(ObstacleCircle(obs, self.avoidance_distance)) - - # combine circles that overlap - i = 0 - while(i < len(obstacle_circles)-1): - j = i+1 - while(j < len(obstacle_circles)): - circ_dist = obstacle_circles[i].distance_to(obstacle_circles[j]) - if circ_dist < max(obstacle_circles[i].radius, obstacle_circles[j].radius): - - circ2 = obstacle_circles.pop(j) - circ1 = obstacle_circles.pop(i) - new_circ_x = (circ1.x + circ2.x)/2 - new_circ_y = (circ1.y + circ2.y)/2 - new_circ_rad = max(circ1.radius, circ2.radius) + circ_dist - - obstacle_circles.append(ObstacleCircle([new_circ_x, new_circ_y], new_circ_rad)) - i = -1 # gets set to 0 by i++ below - break - - else: - j += 1 - i += 1 - - circle_events = self.get_circle_events(path, obstacle_circles) - - for event in circle_events: - start_pt = path[event.start_pt_idx] - end_pt = path[event.end_pt_idx] - - circ_center = event.circle - - start_ang = math.atan2(start_pt[1] - circ_center[1], - start_pt[0] - circ_center[0]) - - end_ang = math.atan2(end_pt[1] - circ_center[1], - end_pt[0] - circ_center[0]) - - # TODO: put in its own function - start_center_slope = (start_pt[1] - circ_center[1]) / (start_pt[0] - circ_center[0]) - tangent_at_start = -1 /start_center_slope - tangent_vec = [1, tangent_at_start] / math.sqrt(1 + tangent_at_start**2) - - chord_vec = np.asarray([end_pt[0] - start_pt[0], end_pt[1] - start_pt[1]]) - chord_vec = chord_vec / np.linalg.norm(chord_vec) - - angle_from_tangent = chord_vec - tangent_vec - - while angle_from_tangent > math.pi: - angle_from_tangent -= math.pi - - while angle_from_tangent < -math.pi: - angle_from_tangent += math.pi - - arc_angle = 2 * angle_from_tangent - end_angle = start_ang + arc_angle - - - # if(start_ang > end_ang): - # if(start_ang - end_ang < math.pi): - # count_up = True - # else: - # if(end_ang - start_ang < math.pi): - # count_up = True - - # just find point that is halfway through arc and use that - - """ - TODO - - - draw perpendicular line to path through circle center - - of 2 intersections perp line makes with circ, use one that is closer to path - - draw dubins from before first int to that point - - draw dubins from that point to after second int - """ - - def get_circle_events(self, path, circles): - - circle_flags = np.zeros(len(circles)) - - circle_events = [] - for i in range(len(circles)): - circle_events.append(None) - - for path_dex, path_pt in enumerate(path): - for i, circ in enumerate(circles): - dist = circ.distance_to(path_pt) - if dist < self.avoidance_distance and not circle_flags[i]: - # circle in range - circle_flags[i] = 1 - circle_events[i] = (CircleEvent(circ, path_dex)) - elif dist > self.avoidance_distance and circle_flags[i]: - # circle out of range - circle_flags[i] = 0 - circle_events[i].end_pt_idx = path_dex - - return circle_events - - def get_obstacle_force(self, state, normal_vec): - - obstacle_force = np.zeros(len(self.obstacles[0])) - - for obs in self.obstacles: - new_force = [] - - dist = np.sum(np.sqrt((state - obs)**2)) - - - force = self.repel_factor / dist**2 - - # print("\t obs dist force the quantity", obs, dist, force, ((obs - state) @ normal_vec)) - - - obstacle_force += force * ((obs - state) @ normal_vec) / np.linalg.norm(obs - state) - # TODO: all lists or all np arrays for inputs - # for dim in range(len(self.obstacles[0])): - # obstacle_force[dim] = (force * (obs[dim] - state[dim])) - - return obstacle_force - - def goal_attraction(self, state, path_point): - - state = np.asarray(state) - - dx = state[0] - path_point[0] - dy = state[1] - path_point[1] - dist = -np.sqrt(dx**2 + dy**2) - - att_force = np.array([dx/dist, dy/dist])*self.DMP_path_attraction - return att_force - @staticmethod def dist_between(p1, p2): return np.linalg.norm(p1 - p2) - def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None): + def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None, title=None): - _, path = self.recreate_trajectory(q0, g, T, None) - - self.obstacles = obstacles - t, pos = self.recreate_trajectory(q0, g, T, path) - state_hist = np.asarray(pos) + t, path = self.recreate_trajectory(q0, g, T) path = np.asarray(path) - # state_hist = np.asarray(pos) - if visualize: if self.training_data.shape[1] == 2: if self.obstacles is not None: @@ -383,19 +182,24 @@ def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None): plt.scatter(obs[0], obs[1], color='k', label='Obstacle') else: plt.scatter(obs[0], obs[1], color='k') + + plt.cla() plt.plot(self.training_data[:,0], self.training_data[:,1], label="Training Data") plt.plot(path[:,0], path[:,1], label="DMP Approximation") - plt.plot(state_hist[:,0], state_hist[:,1], - label="path") + plt.xlim(0,2) + plt.ylim(0,2) plt.xlabel("X Position") plt.ylabel("Y Position") + if title is not None: + plt.title(title) plt.legend() - plt.show() + plt.draw() + plt.pause(0.02) - return t, pos + return t, path def show_DMP_purpose(self): """ @@ -406,65 +210,43 @@ def show_DMP_purpose(self): """ q0_orig = self.training_data[0] g_orig = self.training_data[-1] + T_orig = self.T_orig - t_norm, pos_norm = self.recreate_trajectory(q0_orig, - g_orig, - self.T_orig) - t_fast, pos_fast = self.recreate_trajectory(q0_orig, - g_orig, - self.T_orig/2) - t_close, pos_close = self.recreate_trajectory(q0_orig, - g_orig/2, - self.T_orig) - - plt.plot(self.train_t_vals, self.training_data, - label="Training Data") - plt.plot(t_norm, pos_norm, label="DMP Approximation", - linestyle='--') - plt.plot(t_fast, pos_fast, label="Decreasing time duration", - linestyle='--') - plt.plot(t_close, pos_close, label='Decreasing goal position', - linestyle='--') - plt.xlabel("X Position") - plt.ylabel("Y Position") - plt.legend() - plt.show() + q0_vals = np.vstack([np.linspace(q0_orig, 2*q0_orig, 20), np.linspace(q0_orig, q0_orig/2, 20)]) + g_vals = np.vstack([np.linspace(g_orig, 2*g_orig, 20), np.linspace(g_orig, g_orig/2, 20)]) + T_vals = np.hstack([np.linspace(T_orig, 2*T_orig, 20), np.linspace(T_orig, T_orig/2, 20)]) -if __name__ == '__main__': - period = 2*np.pi - # t = np.arange(0, np.pi/2, 0.01) - # training_data = np.asarray([np.sin(t) + 0.02*np.random.rand(t.shape[0]), - # np.cos(t) + 0.02*np.random.rand(t.shape[0]) ]).T - t = np.arange(0,1,0.01) - training_data = np.asarray([t, np.flip(t,0)]).T + for new_q0_value in q0_vals: + plot_title = "Initial Position = [" + \ + str(round(new_q0_value[0],2)) + ", " \ + + str(round(new_q0_value[1], 2)) + "]" + _, _ = self.solve_trajectory(new_q0_value, g_orig, T_orig, + title=plot_title) - DMP_controller = DMP(training_data, period) - # DMP_controller.show_DMP_purpose() + for new_g_value in g_vals: + plot_title = "Initial Position = [" + \ + str(round(new_g_value[0],2)) + ", " \ + + str(round(new_g_value[1], 2)) + "]" - obs = np.asarray([[0.3, 0.6], [0.7,0.25]]) - # obs = np.asarray([[0.435,0.9], [0.87,0.5]]) - # obs = np.asarray([[0.435,0.8]]) - # obs = np.asarray([[0.7,0.7]]) - DMP_controller.solve_trajectory([0,1], [1,0], 3, obstacles=obs) + _, _ = self.solve_trajectory(q0_orig, new_g_value, T_orig, + title=plot_title) -""" -okay here's how to handle the obstacles: -- draw circle around each obstacle point -- dubins curve from two points before first intersection to furthest point on circle - (create angle with obstacle point as center & the two points [next to intersection points && not inside circle] - and draw bisector of circle_radius, add point with heading tangent to circ there) - -> probably want points to be slightly further than radius length to give dubins some slack to work with -- dubins curve from the two circle tangent points from bisector to the two points right after second intersection -- to handle close obstacles: - for multiple obs in obstacles: - if dist_nearest_obstacle(obs) < turning_radius: - -average two points - - draw larger circle around average (~1.5x max(turning_radius, dist(obs, nearest_obs))) - - remove two points from obs, add larger circ (or maybe just have a separate storage for these) - - next point is skipped in loop - - keep doing above loop until it loops through all points without making a change + print(T_vals) + for new_T_value in T_vals: + plot_title = "Period = " + str(round(new_T_value,2)) + "[sec]" + _, _ = self.solve_trajectory(q0_orig, g_orig, new_T_value, + title=plot_title) +if __name__ == '__main__': + period = 2*np.pi + t = np.arange(0, np.pi/2, 0.01) + training_data = np.asarray([np.sin(t) + 0.02*np.random.rand(t.shape[0]), + np.cos(t) + 0.02*np.random.rand(t.shape[0]) ]).T -""" + DMP_controller = DMP(training_data, period) + + # DMP_controller.solve_trajectory([0,1], [1,0], 3) + + DMP_controller.show_DMP_purpose() From 43a3f654172c028ae886e961ad12b411bd7c3fa7 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Fri, 1 Oct 2021 01:50:29 -0400 Subject: [PATCH 34/52] Moved DMP files to path planning --- .../dynamic_movement_primitives/dynamic_movement_primitives.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {ArmNavigation => PathPlanning}/dynamic_movement_primitives/dynamic_movement_primitives.py (100%) diff --git a/ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py b/PathPlanning/dynamic_movement_primitives/dynamic_movement_primitives.py similarity index 100% rename from ArmNavigation/dynamic_movement_primitives/dynamic_movement_primitives.py rename to PathPlanning/dynamic_movement_primitives/dynamic_movement_primitives.py From dc75fa7943b4d9e533e09c39e8ba80b6a3d6c8e8 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Fri, 1 Oct 2021 01:51:14 -0400 Subject: [PATCH 35/52] changed folder name --- .../dynamic_movement_primitives.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename PathPlanning/{dynamic_movement_primitives => DynamicMovementPrimitives}/dynamic_movement_primitives.py (100%) diff --git a/PathPlanning/dynamic_movement_primitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py similarity index 100% rename from PathPlanning/dynamic_movement_primitives/dynamic_movement_primitives.py rename to PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py From 846fc4759e08edfe0ae6b7d18679712e7f2b5907 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Fri, 1 Oct 2021 02:46:29 -0400 Subject: [PATCH 36/52] Made demo path cooler --- .../dynamic_movement_primitives.py | 26 ++++++++++++------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 5ad62520b3..440fe05184 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -188,8 +188,8 @@ def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None, title=None) label="Training Data") plt.plot(path[:,0], path[:,1], label="DMP Approximation") - plt.xlim(0,2) - plt.ylim(0,2) + # plt.xlim(0,2) + # plt.ylim(0,2) plt.xlabel("X Position") plt.ylabel("Y Position") @@ -212,10 +212,12 @@ def show_DMP_purpose(self): g_orig = self.training_data[-1] T_orig = self.T_orig - q0_vals = np.vstack([np.linspace(q0_orig, 2*q0_orig, 20), np.linspace(q0_orig, q0_orig/2, 20)]) + dist = np.linalg.norm(g_orig - q0_orig) + + q0_vals = np.vstack([np.linspace(q0_orig, dist/4, 20), np.linspace(q0_orig, -dist/10, 20)]) g_vals = np.vstack([np.linspace(g_orig, 2*g_orig, 20), np.linspace(g_orig, g_orig/2, 20)]) - T_vals = np.hstack([np.linspace(T_orig, 2*T_orig, 20), np.linspace(T_orig, T_orig/2, 20)]) + T_vals = np.linspace(T_orig, 2*T_orig, 20) for new_q0_value in q0_vals: plot_title = "Initial Position = [" + \ @@ -225,14 +227,13 @@ def show_DMP_purpose(self): title=plot_title) for new_g_value in g_vals: - plot_title = "Initial Position = [" + \ + plot_title = "Goal Position = [" + \ str(round(new_g_value[0],2)) + ", " \ + str(round(new_g_value[1], 2)) + "]" _, _ = self.solve_trajectory(q0_orig, new_g_value, T_orig, title=plot_title) - print(T_vals) for new_T_value in T_vals: plot_title = "Period = " + str(round(new_T_value,2)) + "[sec]" @@ -240,10 +241,17 @@ def show_DMP_purpose(self): title=plot_title) if __name__ == '__main__': + period = 2*np.pi - t = np.arange(0, np.pi/2, 0.01) - training_data = np.asarray([np.sin(t) + 0.02*np.random.rand(t.shape[0]), - np.cos(t) + 0.02*np.random.rand(t.shape[0]) ]).T + t = np.arange(0, 3*period/4, 0.01) + t1 = np.arange(3*np.pi/2, 2*np.pi, 0.01)[:-1] + t2 = np.arange(0, np.pi/2, 0.01)[:-1] + t3 = np.arange(np.pi, 3*np.pi/2, 0.01) + data_x = t + 0.02*np.random.rand(t.shape[0]) + data_y = np.concatenate([np.cos(t1) + 0.02*np.random.rand(t1.shape[0]), + np.cos(t2) + 0.02*np.random.rand(t2.shape[0]), + np.sin(t3) + 0.02*np.random.rand(t3.shape[0])]) + training_data = np.vstack([data_x, data_y]).T DMP_controller = DMP(training_data, period) From 5ad64e02159ad13b78ea51a8942dd4ee18103525 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 7 Oct 2021 22:36:59 -0400 Subject: [PATCH 37/52] All working and added visualization tools (will remove --- .../dynamic_movement_primitives.py | 96 ++++++++++--------- 1 file changed, 52 insertions(+), 44 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 440fe05184..87a0570804 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -5,10 +5,13 @@ https://www.frontiersin.org/articles/10.3389/fncom.2013.00138/full """ import matplotlib.pyplot as plt +from matplotlib import animation import numpy as np import copy import math +import imageio + class DMP(object): def __init__(self, training_data, data_period, K=156.25, B=25, @@ -37,6 +40,9 @@ def __init__(self, training_data, data_period, K=156.25, B=25, self.T_orig = data_period # self.training_data = training_data + self.images = [] + self.counter = 0 + self.find_basis_functions_weights(training_data, data_period) def find_basis_functions_weights(self, training_data, data_period, num_weights=10): @@ -101,15 +107,12 @@ def find_basis_functions_weights(self, training_data, data_period, num_weights=1 else: self.weights = np.vstack([self.weights, w[0]]) - return self.weights # TODO: neccesary? - def recreate_trajectory(self, init_state, goal_state, T): """ init_state - initial state/position goal_state - goal state/position T - amount of time to travek q0 -> g - path - TODO """ nrBasis = len(self.weights[0]) # number of gaussian basis functions @@ -169,35 +172,29 @@ def recreate_trajectory(self, init_state, goal_state, T): def dist_between(p1, p2): return np.linalg.norm(p1 - p2) - def solve_trajectory(self, q0, g, T, visualize=True, obstacles=None, title=None): + def view_trajectory(self, path, title=None): - t, path = self.recreate_trajectory(q0, g, T) path = np.asarray(path) - if visualize: - if self.training_data.shape[1] == 2: - if self.obstacles is not None: - for i, obs in enumerate(self.obstacles): - if i == 0: - plt.scatter(obs[0], obs[1], color='k', label='Obstacle') - else: - plt.scatter(obs[0], obs[1], color='k') - - plt.cla() - plt.plot(self.training_data[:,0], self.training_data[:,1], - label="Training Data") - plt.plot(path[:,0], path[:,1], - label="DMP Approximation") - # plt.xlim(0,2) - # plt.ylim(0,2) - - plt.xlabel("X Position") - plt.ylabel("Y Position") - if title is not None: - plt.title(title) - plt.legend() - plt.draw() - plt.pause(0.02) + plt.cla() + plt.plot(self.training_data[:,0], self.training_data[:,1], + label="Training Data") + plt.plot(path[:,0], path[:,1], + label="DMP Approximation") + + plt.xlim([-0.5,5]) + plt.ylim([-2,2]) + + plt.xlabel("X Position") + plt.ylabel("Y Position") + if title is not None: + plt.title(title) + plt.legend() + # plt.draw() + # plt.pause(0.02) + print(self.counter) + plt.savefig("{i}.jpg".format(i=self.counter)) + self.counter+=1 return t, path @@ -208,37 +205,44 @@ def show_DMP_purpose(self): and squeeze it in terms of start and stop position or time """ + q0_orig = self.training_data[0] g_orig = self.training_data[-1] T_orig = self.T_orig - dist = np.linalg.norm(g_orig - q0_orig) - - q0_vals = np.vstack([np.linspace(q0_orig, dist/4, 20), np.linspace(q0_orig, -dist/10, 20)]) + q0_right = q0_orig + np.array([1.0,0]) + q0_up = q0_orig + np.array([0,0.5]) + g_left = g_orig - np.array([1.0, 0]) + g_down = g_orig - np.array([0, 0.5]) - g_vals = np.vstack([np.linspace(g_orig, 2*g_orig, 20), np.linspace(g_orig, g_orig/2, 20)]) + q0_vals = np.vstack([np.linspace(q0_orig, q0_right, 20), + np.linspace(q0_orig, q0_up, 20)]) + g_vals = np.vstack([np.linspace(g_orig, g_left, 20), + np.linspace(g_orig, g_down, 20)]) T_vals = np.linspace(T_orig, 2*T_orig, 20) for new_q0_value in q0_vals: plot_title = "Initial Position = [" + \ str(round(new_q0_value[0],2)) + ", " \ + str(round(new_q0_value[1], 2)) + "]" - _, _ = self.solve_trajectory(new_q0_value, g_orig, T_orig, - title=plot_title) + _, path = self.recreate_trajectory(new_q0_value, g_orig, T_orig) + self.view_trajectory(path, title=plot_title) for new_g_value in g_vals: plot_title = "Goal Position = [" + \ str(round(new_g_value[0],2)) + ", " \ + str(round(new_g_value[1], 2)) + "]" - _, _ = self.solve_trajectory(q0_orig, new_g_value, T_orig, - title=plot_title) + _, path = self.recreate_trajectory(q0_orig, new_g_value, T_orig) + self.view_trajectory(path, title=plot_title) + for new_T_value in T_vals: plot_title = "Period = " + str(round(new_T_value,2)) + "[sec]" - _, _ = self.solve_trajectory(q0_orig, g_orig, new_T_value, - title=plot_title) + _, path = self.recreate_trajectory(q0_orig, g_orig, new_T_value) + self.view_trajectory(path, title=plot_title) + if __name__ == '__main__': @@ -248,13 +252,17 @@ def show_DMP_purpose(self): t2 = np.arange(0, np.pi/2, 0.01)[:-1] t3 = np.arange(np.pi, 3*np.pi/2, 0.01) data_x = t + 0.02*np.random.rand(t.shape[0]) - data_y = np.concatenate([np.cos(t1) + 0.02*np.random.rand(t1.shape[0]), - np.cos(t2) + 0.02*np.random.rand(t2.shape[0]), - np.sin(t3) + 0.02*np.random.rand(t3.shape[0])]) + data_y = np.concatenate([np.cos(t1) + 0.1*np.random.rand(t1.shape[0]), + np.cos(t2) + 0.1*np.random.rand(t2.shape[0]), + np.sin(t3) + 0.1*np.random.rand(t3.shape[0])]) training_data = np.vstack([data_x, data_y]).T DMP_controller = DMP(training_data, period) - # DMP_controller.solve_trajectory([0,1], [1,0], 3) - DMP_controller.show_DMP_purpose() + + gif_path = "normal.gif" + frames_path = "{i}.jpg" + with imageio.get_writer(gif_path, mode='I') as writer: + for i in range(DMP_controller.counter): + writer.append_data(imageio.imread(frames_path.format(i=i))) From 74c0dd64da3a9d56594243953e95f39adf23553e Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Fri, 8 Oct 2021 00:09:32 -0400 Subject: [PATCH 38/52] Fixed unit test and handled TODOs --- .../dynamic_movement_primitives.py | 45 +++++++------------ tests/test_dynamic_movement_primitives.py | 39 +++++++--------- 2 files changed, 33 insertions(+), 51 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 87a0570804..ac44e10840 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -10,39 +10,32 @@ import copy import math -import imageio class DMP(object): - def __init__(self, training_data, data_period, K=156.25, B=25, - timesteps=500, repel_factor=0.001): + def __init__(self, training_data, data_period, K=156.25, B=25): """ Arguments: training_data - data in for [(x1,y1), (x2,y2), ...] data_period - amount of time training data covers K and B - spring and damper constants to define DMP behavior - timesteps - number of points in generated trajectories - repel_factor - controls how much path will avoid obstacles """ + self.K = K # virtual spring constant self.B = B # virtual damper coefficient - self.dt = data_period / timesteps - self.timesteps = timesteps + self.timesteps = training_data.shape[0] + self.dt = data_period / self.timesteps self.weights = None # weights used to generate DMP trajectories self.obstacles = None - self.repel_factor = repel_factor self.avoidance_distance = 0.1 self.DMP_path_attraction = 10 self.T_orig = data_period # self.training_data = training_data - self.images = [] - self.counter = 0 - self.find_basis_functions_weights(training_data, data_period) def find_basis_functions_weights(self, training_data, data_period, num_weights=10): @@ -52,12 +45,17 @@ def find_basis_functions_weights(self, training_data, data_period, num_weights=1 data_period [float] - time duration of data """ + if not isinstance(training_data, np.ndarray): + print("Warning: you should input training data as an np.ndarray") + elif training_data.shape[0] < training_data.shape[1]: + print("Warning: you probably need to transpose your training data") + self.training_data = training_data # for plotting dt = data_period / len(training_data) - init_state = training_data[0] # initial pos - goal_state = training_data[-1] # assume goal is reached by end of data + init_state = training_data[0] + goal_state = training_data[-1] # means (C) and std devs (H) of gaussian basis functions C = np.linspace(0, 1, num_weights) @@ -126,7 +124,6 @@ def recreate_trajectory(self, init_state, goal_state, T): # for plotting self.train_t_vals = np.arange(0, T, self.timesteps) - # TODO: is self.period variable used if not isinstance(init_state, np.ndarray): init_state = np.asarray(init_state) @@ -160,13 +157,14 @@ def recreate_trajectory(self, init_state, goal_state, T): qd = qd + qdd * self.dt q = q + qd * self.dt - # TODO: get rid of this + # # TODO: get rid of this if not isinstance(q, list): new_state = q.tolist() positions.append(new_state) - return np.arange(0, self.timesteps * self.dt, self.dt), positions + t = np.arange(0, self.timesteps * self.dt, self.dt) + return t, np.asarray(positions) @staticmethod def dist_between(p1, p2): @@ -180,7 +178,7 @@ def view_trajectory(self, path, title=None): plt.plot(self.training_data[:,0], self.training_data[:,1], label="Training Data") plt.plot(path[:,0], path[:,1], - label="DMP Approximation") + linewidth=2, label="DMP Approximation") plt.xlim([-0.5,5]) plt.ylim([-2,2]) @@ -190,11 +188,8 @@ def view_trajectory(self, path, title=None): if title is not None: plt.title(title) plt.legend() - # plt.draw() - # plt.pause(0.02) - print(self.counter) - plt.savefig("{i}.jpg".format(i=self.counter)) - self.counter+=1 + plt.draw() + plt.pause(0.02) return t, path @@ -260,9 +255,3 @@ def show_DMP_purpose(self): DMP_controller = DMP(training_data, period) DMP_controller.show_DMP_purpose() - - gif_path = "normal.gif" - frames_path = "{i}.jpg" - with imageio.get_writer(gif_path, mode='I') as writer: - for i in range(DMP_controller.counter): - writer.append_data(imageio.imread(frames_path.format(i=i))) diff --git a/tests/test_dynamic_movement_primitives.py b/tests/test_dynamic_movement_primitives.py index bb4f337e2d..a5fdbaf7a8 100644 --- a/tests/test_dynamic_movement_primitives.py +++ b/tests/test_dynamic_movement_primitives.py @@ -1,39 +1,32 @@ import conftest import numpy as np -from ArmNavigation.dynamic_movement_primitives import \ +from PathPlanning.DynamicMovementPrimitives import \ dynamic_movement_primitives - def test_1(): - # test that functions work at all - DMP_controller = dynamic_movement_primitives.DMP() - DMP_controller.solve_trajectory(0, 3, 3, visualize=False) - - -def test_2(): # test that trajectory can be learned from user-passed data - t = np.arange(0, 5, 0.01) + T=5 + t = np.arange(0, T, 0.01) sin_t = np.sin(t) + train_data = np.array([t, sin_t]).T - train_data = [t, sin_t] - DMP_controller = dynamic_movement_primitives.DMP(training_data=train_data) - DMP_controller.solve_trajectory(-3, 3, 4, visualize=False) + DMP_controller = dynamic_movement_primitives.DMP(train_data, T) + DMP_controller.recreate_trajectory(train_data[0], train_data[-1], 4) -def test_3(): +def test_2(): # check that learned trajectory is close to initial - t = np.arange(0, 3*np.pi/2, 0.01) - sin_t = np.sin(t) + T = 3*np.pi/2 + A_noise = 0.02 + t = np.arange(0, T, 0.01) + noisy_sin_t = np.sin(t) + A_noise*np.random.rand(len(t)) + train_data = np.array([t, noisy_sin_t]).T - train_data = [t, sin_t] - DMP_controller = dynamic_movement_primitives.DMP(dt=0.01, - timesteps=len(t), - training_data=train_data) - t, pos = DMP_controller.solve_trajectory(0, -1, 3*np.pi/2, - visualize=False) + DMP_controller = dynamic_movement_primitives.DMP(train_data, T) + t, pos = DMP_controller.recreate_trajectory(train_data[0], train_data[-1], T) - diff = abs(pos - sin_t) - assert(max(diff) < 0.1) + diff = abs(pos[:,1] - noisy_sin_t) + assert(max(diff) < 5*A_noise) if __name__ == '__main__': From 4d922529f1ec5ed03dcd84f2fcdb45626cff7f67 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Fri, 8 Oct 2021 00:18:24 -0400 Subject: [PATCH 39/52] not gonna handle this one --- .../DynamicMovementPrimitives/dynamic_movement_primitives.py | 1 - 1 file changed, 1 deletion(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index ac44e10840..f40a5925b8 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -157,7 +157,6 @@ def recreate_trajectory(self, init_state, goal_state, T): qd = qd + qdd * self.dt q = q + qd * self.dt - # # TODO: get rid of this if not isinstance(q, list): new_state = q.tolist() From ae50183e44edb40f621228ec0a76a6a2fc032e94 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Mon, 25 Oct 2021 03:03:06 -0400 Subject: [PATCH 40/52] Fixing code style issues --- .../dynamic_movement_primitives.py | 33 ++++++++++--------- 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index f40a5925b8..3088c5687e 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -28,10 +28,6 @@ def __init__(self, training_data, data_period, K=156.25, B=25): self.dt = data_period / self.timesteps self.weights = None # weights used to generate DMP trajectories - self.obstacles = None - - self.avoidance_distance = 0.1 - self.DMP_path_attraction = 10 self.T_orig = data_period # self.training_data = training_data @@ -169,7 +165,7 @@ def recreate_trajectory(self, init_state, goal_state, T): def dist_between(p1, p2): return np.linalg.norm(p1 - p2) - def view_trajectory(self, path, title=None): + def view_trajectory(self, path, title=None, demo=False): path = np.asarray(path) @@ -179,16 +175,21 @@ def view_trajectory(self, path, title=None): plt.plot(path[:,0], path[:,1], linewidth=2, label="DMP Approximation") - plt.xlim([-0.5,5]) - plt.ylim([-2,2]) plt.xlabel("X Position") plt.ylabel("Y Position") + plt.legend() + if title is not None: plt.title(title) - plt.legend() - plt.draw() - plt.pause(0.02) + + if demo: + plt.xlim([-0.5,5]) + plt.ylim([-2,2]) + plt.draw() + plt.pause(0.02) + else: + plt.show() return t, path @@ -220,7 +221,7 @@ def show_DMP_purpose(self): str(round(new_q0_value[0],2)) + ", " \ + str(round(new_q0_value[1], 2)) + "]" _, path = self.recreate_trajectory(new_q0_value, g_orig, T_orig) - self.view_trajectory(path, title=plot_title) + self.view_trajectory(path, title=plot_title, demo=True) for new_g_value in g_vals: plot_title = "Goal Position = [" + \ @@ -228,20 +229,19 @@ def show_DMP_purpose(self): + str(round(new_g_value[1], 2)) + "]" _, path = self.recreate_trajectory(q0_orig, new_g_value, T_orig) - self.view_trajectory(path, title=plot_title) + self.view_trajectory(path, title=plot_title, demo=True) for new_T_value in T_vals: plot_title = "Period = " + str(round(new_T_value,2)) + "[sec]" _, path = self.recreate_trajectory(q0_orig, g_orig, new_T_value) - self.view_trajectory(path, title=plot_title) + self.view_trajectory(path, title=plot_title, demo=True) if __name__ == '__main__': - period = 2*np.pi - t = np.arange(0, 3*period/4, 0.01) + t = np.arange(0, 3*np.pi/2, 0.01) t1 = np.arange(3*np.pi/2, 2*np.pi, 0.01)[:-1] t2 = np.arange(0, np.pi/2, 0.01)[:-1] t3 = np.arange(np.pi, 3*np.pi/2, 0.01) @@ -251,6 +251,9 @@ def show_DMP_purpose(self): np.sin(t3) + 0.1*np.random.rand(t3.shape[0])]) training_data = np.vstack([data_x, data_y]).T + period = 3*np.pi/2 DMP_controller = DMP(training_data, period) DMP_controller.show_DMP_purpose() + # _, path = DMP_controller.recreate_trajectory(training_data[0], training_data[-1], period) + # DMP_controller.view_trajectory(path) From c61b9b048937cc5444d4c78fc529321b06ac5945 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Mon, 25 Oct 2021 00:31:57 -0400 Subject: [PATCH 41/52] more styling --- .../dynamic_movement_primitives.py | 46 ++++++++++--------- 1 file changed, 25 insertions(+), 21 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 3088c5687e..3a00390501 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -4,6 +4,8 @@ https://arxiv.org/abs/2102.03861 https://www.frontiersin.org/articles/10.3389/fncom.2013.00138/full """ + + import matplotlib.pyplot as plt from matplotlib import animation import numpy as np @@ -18,7 +20,7 @@ def __init__(self, training_data, data_period, K=156.25, B=25): Arguments: training_data - data in for [(x1,y1), (x2,y2), ...] data_period - amount of time training data covers - K and B - spring and damper constants to define DMP behavior + K and B - spring and damper constants to define DMP behavior """ self.K = K # virtual spring constant @@ -34,7 +36,8 @@ def __init__(self, training_data, data_period, K=156.25, B=25): self.find_basis_functions_weights(training_data, data_period) - def find_basis_functions_weights(self, training_data, data_period, num_weights=10): + def find_basis_functions_weights(self, training_data, data_period, + num_weights=10): """ Arguments: data [(steps x spacial dim) np array] - data to replicate with DMP @@ -46,7 +49,7 @@ def find_basis_functions_weights(self, training_data, data_period, num_weights=1 elif training_data.shape[0] < training_data.shape[1]: print("Warning: you probably need to transpose your training data") - self.training_data = training_data # for plotting + self.training_data = training_data dt = data_period / len(training_data) @@ -57,9 +60,9 @@ def find_basis_functions_weights(self, training_data, data_period, num_weights=1 C = np.linspace(0, 1, num_weights) H = (0.65*(1./(num_weights-1))**2) - for dim in range(len(training_data[0])): + for dim, _ in enumerate(training_data[0]): - dimension_data = training_data[:,dim] + dimension_data = training_data[:, dim] q0 = init_state[dim] g = goal_state[dim] @@ -101,7 +104,6 @@ def find_basis_functions_weights(self, training_data, data_period, num_weights=1 else: self.weights = np.vstack([self.weights, w[0]]) - def recreate_trajectory(self, init_state, goal_state, T): """ init_state - initial state/position @@ -148,7 +150,9 @@ def recreate_trajectory(self, init_state, goal_state, T): f = 0 # simulate dynamics - qdd[dim] = self.K*(goal_state[dim] - q[dim])/T**2 - self.B*qd[dim]/T + (goal_state[dim] - init_state[dim])*f/T**2 + qdd[dim] = self.K*(goal_state[dim] - q[dim])/T**2 \ + - self.B*qd[dim]/T \ + + (goal_state[dim] - init_state[dim])*f/T**2 qd = qd + qdd * self.dt q = q + qd * self.dt @@ -170,12 +174,11 @@ def view_trajectory(self, path, title=None, demo=False): path = np.asarray(path) plt.cla() - plt.plot(self.training_data[:,0], self.training_data[:,1], + plt.plot(self.training_data[:, 0], self.training_data[:, 1], label="Training Data") - plt.plot(path[:,0], path[:,1], + plt.plot(path[:, 0], path[:, 1], linewidth=2, label="DMP Approximation") - plt.xlabel("X Position") plt.ylabel("Y Position") plt.legend() @@ -184,8 +187,8 @@ def view_trajectory(self, path, title=None, demo=False): plt.title(title) if demo: - plt.xlim([-0.5,5]) - plt.ylim([-2,2]) + plt.xlim([-0.5, 5]) + plt.ylim([-2, 2]) plt.draw() plt.pause(0.02) else: @@ -205,8 +208,11 @@ def show_DMP_purpose(self): g_orig = self.training_data[-1] T_orig = self.T_orig - q0_right = q0_orig + np.array([1.0,0]) - q0_up = q0_orig + np.array([0,0.5]) + # TODO: make this scale + # max = np.amax() + + q0_right = q0_orig + np.array([1.0, 0]) + q0_up = q0_orig + np.array([0, 0.5]) g_left = g_orig - np.array([1.0, 0]) g_down = g_orig - np.array([0, 0.5]) @@ -218,22 +224,21 @@ def show_DMP_purpose(self): for new_q0_value in q0_vals: plot_title = "Initial Position = [" + \ - str(round(new_q0_value[0],2)) + ", " \ + str(round(new_q0_value[0], 2)) + ", " \ + str(round(new_q0_value[1], 2)) + "]" _, path = self.recreate_trajectory(new_q0_value, g_orig, T_orig) self.view_trajectory(path, title=plot_title, demo=True) for new_g_value in g_vals: plot_title = "Goal Position = [" + \ - str(round(new_g_value[0],2)) + ", " \ + str(round(new_g_value[0], 2)) + ", " \ + str(round(new_g_value[1], 2)) + "]" _, path = self.recreate_trajectory(q0_orig, new_g_value, T_orig) self.view_trajectory(path, title=plot_title, demo=True) - for new_T_value in T_vals: - plot_title = "Period = " + str(round(new_T_value,2)) + "[sec]" + plot_title = "Period = " + str(round(new_T_value, 2)) + "[sec]" _, path = self.recreate_trajectory(q0_orig, g_orig, new_T_value) self.view_trajectory(path, title=plot_title, demo=True) @@ -247,13 +252,12 @@ def show_DMP_purpose(self): t3 = np.arange(np.pi, 3*np.pi/2, 0.01) data_x = t + 0.02*np.random.rand(t.shape[0]) data_y = np.concatenate([np.cos(t1) + 0.1*np.random.rand(t1.shape[0]), - np.cos(t2) + 0.1*np.random.rand(t2.shape[0]), - np.sin(t3) + 0.1*np.random.rand(t3.shape[0])]) + np.cos(t2) + 0.1*np.random.rand(t2.shape[0]), + np.sin(t3) + 0.1*np.random.rand(t3.shape[0])]) training_data = np.vstack([data_x, data_y]).T period = 3*np.pi/2 DMP_controller = DMP(training_data, period) DMP_controller.show_DMP_purpose() - # _, path = DMP_controller.recreate_trajectory(training_data[0], training_data[-1], period) # DMP_controller.view_trajectory(path) From 3fab1d9c0999d8a5d6cb70622f3da33941e6b32b Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Mon, 25 Oct 2021 00:35:31 -0400 Subject: [PATCH 42/52] demo now scales with data --- .../dynamic_movement_primitives.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 3a00390501..6ac17bd40f 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -208,13 +208,13 @@ def show_DMP_purpose(self): g_orig = self.training_data[-1] T_orig = self.T_orig - # TODO: make this scale - # max = np.amax() + data_range = (np.amax(self.training_data[:, 0]) \ + - np.amin(self.training_data[:, 0])) / 4 - q0_right = q0_orig + np.array([1.0, 0]) - q0_up = q0_orig + np.array([0, 0.5]) - g_left = g_orig - np.array([1.0, 0]) - g_down = g_orig - np.array([0, 0.5]) + q0_right = q0_orig + np.array([data_range, 0]) + q0_up = q0_orig + np.array([0, data_range/2]) + g_left = g_orig - np.array([data_range, 0]) + g_down = g_orig - np.array([0, data_range/2]) q0_vals = np.vstack([np.linspace(q0_orig, q0_right, 20), np.linspace(q0_orig, q0_up, 20)]) From c18242e7b8efc8d1b667f6937e5cfa28c95a6457 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Mon, 25 Oct 2021 00:43:29 -0400 Subject: [PATCH 43/52] CI errors --- .../dynamic_movement_primitives.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 6ac17bd40f..f9680f19c6 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -18,7 +18,7 @@ class DMP(object): def __init__(self, training_data, data_period, K=156.25, B=25): """ Arguments: - training_data - data in for [(x1,y1), (x2,y2), ...] + training_data - input data of form [N, dim] data_period - amount of time training data covers K and B - spring and damper constants to define DMP behavior """ @@ -151,8 +151,8 @@ def recreate_trajectory(self, init_state, goal_state, T): # simulate dynamics qdd[dim] = self.K*(goal_state[dim] - q[dim])/T**2 \ - - self.B*qd[dim]/T \ - + (goal_state[dim] - init_state[dim])*f/T**2 + - self.B*qd[dim]/T \ + + (goal_state[dim] - init_state[dim])*f/T**2 qd = qd + qdd * self.dt q = q + qd * self.dt @@ -208,7 +208,7 @@ def show_DMP_purpose(self): g_orig = self.training_data[-1] T_orig = self.T_orig - data_range = (np.amax(self.training_data[:, 0]) \ + data_range = (np.amax(self.training_data[:, 0]) - np.amin(self.training_data[:, 0])) / 4 q0_right = q0_orig + np.array([data_range, 0]) From 9fefe20f069dbb32af77f2296d43831f2c1cb5da Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Mon, 25 Oct 2021 00:48:58 -0400 Subject: [PATCH 44/52] CI errors --- .../dynamic_movement_primitives.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index f9680f19c6..3f10476ffb 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -3,10 +3,11 @@ More information on Dynamic Movement Primitives available at: https://arxiv.org/abs/2102.03861 https://www.frontiersin.org/articles/10.3389/fncom.2013.00138/full + """ -import matplotlib.pyplot as plt +from matplotlib import pyplot as plt from matplotlib import animation import numpy as np import copy @@ -20,7 +21,8 @@ def __init__(self, training_data, data_period, K=156.25, B=25): Arguments: training_data - input data of form [N, dim] data_period - amount of time training data covers - K and B - spring and damper constants to define DMP behavior + K and B - spring and damper constants to define + DMP behavior """ self.K = K # virtual spring constant @@ -151,8 +153,8 @@ def recreate_trajectory(self, init_state, goal_state, T): # simulate dynamics qdd[dim] = self.K*(goal_state[dim] - q[dim])/T**2 \ - - self.B*qd[dim]/T \ - + (goal_state[dim] - init_state[dim])*f/T**2 + - self.B*qd[dim]/T \ + + (goal_state[dim] - init_state[dim])*f/T**2 qd = qd + qdd * self.dt q = q + qd * self.dt From 342dc2561f9608f964e9ee6abdc0d7fce379b2f7 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Mon, 25 Oct 2021 03:04:08 -0400 Subject: [PATCH 45/52] fixing CI errors --- .../DynamicMovementPrimitives/dynamic_movement_primitives.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 3f10476ffb..fab9679dee 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -153,8 +153,8 @@ def recreate_trajectory(self, init_state, goal_state, T): # simulate dynamics qdd[dim] = self.K*(goal_state[dim] - q[dim])/T**2 \ - - self.B*qd[dim]/T \ - + (goal_state[dim] - init_state[dim])*f/T**2 + - self.B*qd[dim]/T \ + + (goal_state[dim] - init_state[dim])*f/T**2 qd = qd + qdd * self.dt q = q + qd * self.dt From fc6f5b063ff06e36e5ad6130ffb1940fa7db5806 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Mon, 25 Oct 2021 12:27:06 -0400 Subject: [PATCH 46/52] formatting --- .../dynamic_movement_primitives.py | 6 +++--- tests/test_dynamic_movement_primitives.py | 8 +++++--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index fab9679dee..e05cbc230e 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -152,9 +152,9 @@ def recreate_trajectory(self, init_state, goal_state, T): f = 0 # simulate dynamics - qdd[dim] = self.K*(goal_state[dim] - q[dim])/T**2 \ - - self.B*qd[dim]/T \ - + (goal_state[dim] - init_state[dim])*f/T**2 + qdd[dim] = (self.K*(goal_state[dim] - q[dim])/T**2 + - self.B*qd[dim]/T + + (goal_state[dim] - init_state[dim])*f/T**2) qd = qd + qdd * self.dt q = q + qd * self.dt diff --git a/tests/test_dynamic_movement_primitives.py b/tests/test_dynamic_movement_primitives.py index a5fdbaf7a8..6bf33aedd5 100644 --- a/tests/test_dynamic_movement_primitives.py +++ b/tests/test_dynamic_movement_primitives.py @@ -3,9 +3,10 @@ from PathPlanning.DynamicMovementPrimitives import \ dynamic_movement_primitives + def test_1(): # test that trajectory can be learned from user-passed data - T=5 + T = 5 t = np.arange(0, T, 0.01) sin_t = np.sin(t) train_data = np.array([t, sin_t]).T @@ -23,9 +24,10 @@ def test_2(): train_data = np.array([t, noisy_sin_t]).T DMP_controller = dynamic_movement_primitives.DMP(train_data, T) - t, pos = DMP_controller.recreate_trajectory(train_data[0], train_data[-1], T) + t, pos = DMP_controller.recreate_trajectory(train_data[0], + train_data[-1], T) - diff = abs(pos[:,1] - noisy_sin_t) + diff = abs(pos[:, 1] - noisy_sin_t) assert(max(diff) < 5*A_noise) From 4b1eca4811a4d4b86e971f2ae31f259046ab61de Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 30 Oct 2021 19:18:37 -0400 Subject: [PATCH 47/52] Removed dead code --- .../DynamicMovementPrimitives/dynamic_movement_primitives.py | 1 - 1 file changed, 1 deletion(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index e05cbc230e..903ce916dd 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -262,4 +262,3 @@ def show_DMP_purpose(self): DMP_controller = DMP(training_data, period) DMP_controller.show_DMP_purpose() - # DMP_controller.view_trajectory(path) From 4255d32e2260756c12246618e45d20374b36dd1e Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 30 Oct 2021 19:21:08 -0400 Subject: [PATCH 48/52] removed unused imports --- .../DynamicMovementPrimitives/dynamic_movement_primitives.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 903ce916dd..e9df88dda0 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -8,10 +8,7 @@ from matplotlib import pyplot as plt -from matplotlib import animation import numpy as np -import copy -import math class DMP(object): From 0457c61ee31873f83b0534686eef1fffee8a9ba7 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 30 Oct 2021 19:22:39 -0400 Subject: [PATCH 49/52] removed uneccesary initialization --- .../DynamicMovementPrimitives/dynamic_movement_primitives.py | 1 - 1 file changed, 1 deletion(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index e9df88dda0..16cf06222a 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -130,7 +130,6 @@ def recreate_trajectory(self, init_state, goal_state, T): q = init_state dimensions = self.weights.shape[0] qd = np.zeros(dimensions) - qdd = np.zeros(dimensions) positions = [] for k in range(self.timesteps): From 342eb0398d660aa480975d7ae3f48670a49ead9e Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 3 Nov 2021 17:02:02 -0400 Subject: [PATCH 50/52] Applying PR feedback --- .../dynamic_movement_primitives.py | 68 +++++++++---------- tests/test_dynamic_movement_primitives.py | 13 ++++ 2 files changed, 46 insertions(+), 35 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 16cf06222a..09c81986fb 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -31,8 +31,8 @@ def __init__(self, training_data, data_period, K=156.25, B=25): self.weights = None # weights used to generate DMP trajectories self.T_orig = data_period - # self.training_data = training_data + self.training_data = training_data self.find_basis_functions_weights(training_data, data_period) def find_basis_functions_weights(self, training_data, data_period, @@ -48,8 +48,6 @@ def find_basis_functions_weights(self, training_data, data_period, elif training_data.shape[0] < training_data.shape[1]: print("Warning: you probably need to transpose your training data") - self.training_data = training_data - dt = data_period / len(training_data) init_state = training_data[0] @@ -78,16 +76,16 @@ def find_basis_functions_weights(self, training_data, data_period, else: qd = (dimension_data[i+1] - dimension_data[i]) / dt - Phi = [np.exp(-0.5 * ((i * dt / data_period) - c)**2 / H) + phi = [np.exp(-0.5 * ((i * dt / data_period) - c)**2 / H) for c in C] - Phi = Phi/np.sum(Phi) + phi = phi/np.sum(phi) qdd = (qd - qd_last)/dt f = (qdd * data_period**2 - self.K * (g - q) + self.B * qd * data_period) / (g - q0) - phi_vals.append(Phi) + phi_vals.append(phi) f_vals.append(f) qd_last = qd @@ -107,7 +105,7 @@ def recreate_trajectory(self, init_state, goal_state, T): """ init_state - initial state/position goal_state - goal state/position - T - amount of time to travek q0 -> g + T - amount of time to travel q0 -> g """ nrBasis = len(self.weights[0]) # number of gaussian basis functions @@ -117,33 +115,28 @@ def recreate_trajectory(self, init_state, goal_state, T): H = (0.65*(1./(nrBasis-1))**2) # initialize virtual system - t = 0 + time = 0 # for plotting self.train_t_vals = np.arange(0, T, self.timesteps) - if not isinstance(init_state, np.ndarray): - init_state = np.asarray(init_state) - if not isinstance(goal_state, np.ndarray): - goal_state = np.asarray(goal_state) - q = init_state dimensions = self.weights.shape[0] qd = np.zeros(dimensions) - positions = [] + positions = np.array([]) for k in range(self.timesteps): new_state = [] - t = t + self.dt + time = time + self.dt qdd = np.zeros(dimensions) for dim in range(dimensions): - if t <= T: - Phi = [np.exp(-0.5 * ((t / T) - c)**2 / H) for c in C] - Phi = Phi / np.sum(Phi) - f = np.dot(Phi, self.weights[dim]) + if time <= T: + phi = [np.exp(-0.5 * ((time / T) - c)**2 / H) for c in C] + phi = phi / np.sum(phi) + f = np.dot(phi, self.weights[dim]) else: f = 0 @@ -155,13 +148,13 @@ def recreate_trajectory(self, init_state, goal_state, T): qd = qd + qdd * self.dt q = q + qd * self.dt - if not isinstance(q, list): - new_state = q.tolist() - - positions.append(new_state) + if positions.size == 0: + positions = q + else: + positions = np.vstack([positions, q]) t = np.arange(0, self.timesteps * self.dt, self.dt) - return t, np.asarray(positions) + return t, positions @staticmethod def dist_between(p1, p2): @@ -192,8 +185,6 @@ def view_trajectory(self, path, title=None, demo=False): else: plt.show() - return t, path - def show_DMP_purpose(self): """ This function conveys the purpose of DMPs: @@ -221,29 +212,31 @@ def show_DMP_purpose(self): T_vals = np.linspace(T_orig, 2*T_orig, 20) for new_q0_value in q0_vals: - plot_title = "Initial Position = [" + \ - str(round(new_q0_value[0], 2)) + ", " \ - + str(round(new_q0_value[1], 2)) + "]" + plot_title = "Initial Position = [%s, %s]" % \ + (round(new_q0_value[0], 2), round(new_q0_value[1], 2)) + _, path = self.recreate_trajectory(new_q0_value, g_orig, T_orig) self.view_trajectory(path, title=plot_title, demo=True) for new_g_value in g_vals: - plot_title = "Goal Position = [" + \ - str(round(new_g_value[0], 2)) + ", " \ - + str(round(new_g_value[1], 2)) + "]" + plot_title = "Goal Position = [%s, %s]" % \ + (round(new_g_value[0], 2), round(new_g_value[1], 2)) _, path = self.recreate_trajectory(q0_orig, new_g_value, T_orig) self.view_trajectory(path, title=plot_title, demo=True) for new_T_value in T_vals: - plot_title = "Period = " + str(round(new_T_value, 2)) + "[sec]" + plot_title = "Period = %s [sec]" % round(new_T_value, 2) _, path = self.recreate_trajectory(q0_orig, g_orig, new_T_value) self.view_trajectory(path, title=plot_title, demo=True) -if __name__ == '__main__': - +def example_DMP(): + """ + Creates a noisy trajectory, fits weights to it, and then adjusts the + trajectory by moving its start position, goal position, or period + """ t = np.arange(0, 3*np.pi/2, 0.01) t1 = np.arange(3*np.pi/2, 2*np.pi, 0.01)[:-1] t2 = np.arange(0, np.pi/2, 0.01)[:-1] @@ -258,3 +251,8 @@ def show_DMP_purpose(self): DMP_controller = DMP(training_data, period) DMP_controller.show_DMP_purpose() + + +if __name__ == '__main__': + + example_DMP() diff --git a/tests/test_dynamic_movement_primitives.py b/tests/test_dynamic_movement_primitives.py index 6bf33aedd5..d268bc44f3 100644 --- a/tests/test_dynamic_movement_primitives.py +++ b/tests/test_dynamic_movement_primitives.py @@ -16,6 +16,19 @@ def test_1(): def test_2(): + # test that length of trajectory is equal to desired number of timesteps + T = 5 + t = np.arange(0, T, 0.01) + sin_t = np.sin(t) + train_data = np.array([t, sin_t]).T + + DMP_controller = dynamic_movement_primitives.DMP(train_data, T) + t, path = DMP_controller.recreate_trajectory(train_data[0], train_data[-1], 4) + + assert(path.shape[0] == DMP_controller.timesteps) + + +def test_3(): # check that learned trajectory is close to initial T = 3*np.pi/2 A_noise = 0.02 From 70a495d79549cc6c1108d56820c8dc1213514b92 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 3 Nov 2021 17:32:48 -0400 Subject: [PATCH 51/52] fixing CI errors --- .../DynamicMovementPrimitives/dynamic_movement_primitives.py | 1 - tests/test_dynamic_movement_primitives.py | 3 ++- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 09c81986fb..92b7cae319 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -126,7 +126,6 @@ def recreate_trajectory(self, init_state, goal_state, T): positions = np.array([]) for k in range(self.timesteps): - new_state = [] time = time + self.dt qdd = np.zeros(dimensions) diff --git a/tests/test_dynamic_movement_primitives.py b/tests/test_dynamic_movement_primitives.py index d268bc44f3..3ab1c8a32c 100644 --- a/tests/test_dynamic_movement_primitives.py +++ b/tests/test_dynamic_movement_primitives.py @@ -23,7 +23,8 @@ def test_2(): train_data = np.array([t, sin_t]).T DMP_controller = dynamic_movement_primitives.DMP(train_data, T) - t, path = DMP_controller.recreate_trajectory(train_data[0], train_data[-1], 4) + t, path = DMP_controller.recreate_trajectory(train_data[0], + train_data[-1], 4) assert(path.shape[0] == DMP_controller.timesteps) From c2ac3ab7c3f3c66a9caa654d024e7058eb41010c Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Thu, 11 Nov 2021 11:20:13 -0500 Subject: [PATCH 52/52] added description to header and removed unused variable --- .../dynamic_movement_primitives.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py index 92b7cae319..468d3b9f97 100644 --- a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py +++ b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py @@ -1,5 +1,12 @@ """ Author: Jonathan Schwartz (github.com/SchwartzCode) + +This code provides a simple implementation of Dynamic Movement +Primitives, which is an approach to learning curves by modelling +them as a weighted sum of gaussian distributions. This approach +can be used to dampen noise in a curve, and can also be used to +stretch a curve by adjusting its start and end points. + More information on Dynamic Movement Primitives available at: https://arxiv.org/abs/2102.03861 https://www.frontiersin.org/articles/10.3389/fncom.2013.00138/full @@ -117,9 +124,6 @@ def recreate_trajectory(self, init_state, goal_state, T): # initialize virtual system time = 0 - # for plotting - self.train_t_vals = np.arange(0, T, self.timesteps) - q = init_state dimensions = self.weights.shape[0] qd = np.zeros(dimensions)