Dynamic Movement Primitives Implementation (AtsushiSakai#526)

SchwartzCode · web-flow · commit 0df55e943e66 · 2021-11-12T19:28:16.000+09:00
* 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

* Created test script for dubins path generator

* Made len == 0 it's own case, also changed 'l' to 'len' to appease travisCI

* More variable renaming to appease CI

* Broke == 0 into its own case in dubins planner, also Renaming files to appease CI

* Reverting some naming changes

* Turns out theres already a test for dubins.. not sure how I missed that

* Note to self: run the test cases on your own before throwing them at CI

* Added handling of length=0 case in generate_local_course()

* Missed reverting 'mode' back to 'm' in one spot

* Addressing style issues (line length)

* Mostly works, now just need to setup linear regression to solve for weights

* Re-arranged class

* Wrote DMP program and added tests file

* Styling fixes

* More styling

* Missed one indent

* Multi-dimension path learning (e.g. in x and y instead of just x)

* Added potential field obstacle avoidance

* Potential field working much better but has issues with reaching goal state

* Path ending to short not a result of obstacles, should be fix-able

* Mostly working! end won't go to goal

* split DMP and path following

* pretty close

* Okay this is working pretty well

* looks.. okay. was using the wrong vector before

* a plan to fix this mess

* Okay seriously going to pivot to the dubins approach im done with potential field lol

* Finished obstacle circle handling (and merging circles that are closer than their radii)

* Finished circle event finder function

* Some progress in preparing for dubins curves

* Finished angle finding algo, need to test

* Okay getting back to this, going to ignore the navigation and just focus on path generation since that's what DMP is for

* Moved DMP files to path planning

* changed folder name

* Made demo path cooler

* All working and added visualization tools (will remove

* Fixed unit test and handled TODOs

* not gonna handle this one

* demo now scales with data

* CI errors

* CI errors

* Fixing code style issues

* more styling

* fixing CI errors

* formatting

* Removed dead code

* removed unused imports

* removed uneccesary initialization

* Applying PR feedback

* fixing CI errors

* added description to header and removed unused variable
diff --git a/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py b/PathPlanning/DynamicMovementPrimitives/dynamic_movement_primitives.py
@@ -0,0 +1,261 @@
+"""
+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
+
+"""
+
+
+from matplotlib import pyplot as plt
+import numpy as np
+
+
+class DMP(object):
+
+    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
+        """
+
+        self.K = K  # virtual spring constant
+        self.B = B  # virtual damper coefficient
+
+        self.timesteps = training_data.shape[0]
+        self.dt = data_period / self.timesteps
+
+        self.weights = None  # weights used to generate DMP trajectories
+
+        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=10):
+        """
+        Arguments:
+            data [(steps x spacial dim) np array] - data to replicate with DMP
+            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")
+
+        dt = data_period / len(training_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)
+        H = (0.65*(1./(num_weights-1))**2)
+
+        for dim, _ in enumerate(training_data[0]):
+
+            dimension_data = training_data[:, dim]
+
+            q0 = init_state[dim]
+            g = goal_state[dim]
+
+            q = q0
+            qd_last = 0
+
+            phi_vals = []
+            f_vals = []
+
+            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 / data_period) - c)**2 / H)
+                       for c in C]
+                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)
+                f_vals.append(f)
+
+                qd_last = qd
+                q += qd * dt
+
+            phi_vals = np.asarray(phi_vals)
+            f_vals = np.asarray(f_vals)
+
+            w = np.linalg.lstsq(phi_vals, f_vals, rcond=None)
+
+            if self.weights is None:
+                self.weights = np.asarray(w[0])
+            else:
+                self.weights = np.vstack([self.weights, w[0]])
+
+    def recreate_trajectory(self, init_state, goal_state, T):
+        """
+        init_state - initial state/position
+        goal_state - goal state/position
+        T  - amount of time to travel q0 -> g
+        """
+
+        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
+        time = 0
+
+        q = init_state
+        dimensions = self.weights.shape[0]
+        qd = np.zeros(dimensions)
+
+        positions = np.array([])
+        for k in range(self.timesteps):
+            time = time + self.dt
+
+            qdd = np.zeros(dimensions)
+
+            for dim in range(dimensions):
+
+                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
+
+                # 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
+
+            if positions.size == 0:
+                positions = q
+            else:
+                positions = np.vstack([positions, q])
+
+        t = np.arange(0, self.timesteps * self.dt, self.dt)
+        return t, positions
+
+    @staticmethod
+    def dist_between(p1, p2):
+        return np.linalg.norm(p1 - p2)
+
+    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],
+                 label="Training Data")
+        plt.plot(path[:, 0], path[:, 1],
+                 linewidth=2, label="DMP Approximation")
+
+        plt.xlabel("X Position")
+        plt.ylabel("Y Position")
+        plt.legend()
+
+        if title is not None:
+            plt.title(title)
+
+        if demo:
+            plt.xlim([-0.5, 5])
+            plt.ylim([-2, 2])
+            plt.draw()
+            plt.pause(0.02)
+        else:
+            plt.show()
+
+    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_orig = self.T_orig
+
+        data_range = (np.amax(self.training_data[:, 0])
+                      - np.amin(self.training_data[:, 0])) / 4
+
+        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)])
+        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 = [%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 = [%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 = %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)
+
+
+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]
+    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])])
+    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()
+
+
+if __name__ == '__main__':
+
+    example_DMP()
diff --git a/tests/test_dynamic_movement_primitives.py b/tests/test_dynamic_movement_primitives.py
@@ -0,0 +1,49 @@
+import conftest
+import numpy as np
+from PathPlanning.DynamicMovementPrimitives import \
+            dynamic_movement_primitives
+
+
+def test_1():
+    # test that trajectory can be learned from user-passed data
+    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)
+    DMP_controller.recreate_trajectory(train_data[0], train_data[-1], 4)
+
+
+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
+    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
+
+    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[:, 1] - noisy_sin_t)
+    assert(max(diff) < 5*A_noise)
+
+
+if __name__ == '__main__':
+    conftest.run_this_test(__file__)
