Diff of /arm_model/analysis.py [000000] .. [17a672]

Switch to side-by-side view

--- a
+++ b/arm_model/analysis.py
@@ -0,0 +1,173 @@
+import numpy as np
+import pylab as plt
+import matplotlib as mpl
+# from mpl_toolkits.mplot3d import Axes3D
+import pandas as pd
+from util import to_np_mat, plot_corr_ellipses,  \
+    convex_bounded_vertex_enumeration, nullspace
+from logger import Logger
+
+
+# ------------------------------------------------------------------------
+# FeasibleMuscleSetAnalysis
+# ------------------------------------------------------------------------
+
+def construct_muscle_space_inequality(NR, fm_par, Fmax):
+    """Construct the feasible muscle space Z f_m0 <= B .
+
+    Parameters
+    ----------
+
+    NR: moment arm null space matrix
+
+    fm_par: particular muscle forces
+
+    Fmax: maximum muscle force
+
+    """
+    Z0 = -NR
+    Z1 = NR
+    B0 = fm_par
+    B1 = np.asmatrix(np.diag(Fmax)).reshape(Fmax.shape[0], 1) - fm_par
+    Z = np.concatenate((Z0, Z1), axis=0)
+    B = np.concatenate((B0, B1), axis=0)
+    return Z, B
+
+
+class FeasibleMuscleSetAnalysis:
+    """Feasible muscle set analysis.
+
+    The required command along with the state of the system are recorded. Then
+    this information is used to compute the feasible muscle null space and
+    visualize it.
+
+    """
+
+    def __init__(self, model, simulation_reporter):
+        """
+        """
+        self.logger = Logger('FeasibleMuscleSetAnalsysis')
+        self.model = model
+        self.simulation_reporter = simulation_reporter
+
+    def visualize_simple_muscle(self, t, ax=None):
+        """Visualize the feasible force set at a particular time instance for a linear
+        muscle.
+
+        Parameters
+        ----------
+
+        t: time
+
+        ax: 1 x 3 axis
+
+        """
+        m = self.model.md
+        q, Z, B, NR, fm_par = self.calculate_simple_muscles(t)
+        x_max = np.max(to_np_mat(self.model.Fmax))
+        fm_set = self.generate_solutions(Z, B, NR, fm_par)
+        dataframe = pd.DataFrame(fm_set, columns=['$m_' + str(i) + '$' for i in
+                                                  range(1, m + 1)])
+
+        # box plot
+        if ax is None or ax.shape[0] < 3:
+            fig, ax = plt.subplots(1, 3, figsize=(15, 5))
+
+        # box plot
+        dataframe.plot.box(ax=ax[0])
+        ax[0].set_xlabel('muscle id')
+        ax[0].set_ylabel('force $(N)$')
+        ax[0].set_title('Muscle-Force Box Plot')
+        ax[0].set_ylim([0, 1.1 * x_max])
+
+        # correlation matrix
+        cmap = mpl.cm.jet
+        norm = mpl.colors.Normalize(vmin=-1, vmax=1)
+        corr = dataframe.corr()
+        m = plot_corr_ellipses(corr, ax=ax[1], norm=norm, cmap=cmap)
+        cb = plt.colorbar(m, ax=ax[1], orientation='vertical', norm=norm,
+                          cmap=cmap)
+        cb.set_label('Correlation Coefficient')
+        ax[1].margins(0.1)
+        ax[1].set_xlabel('muscle id')
+        ax[1].set_ylabel('muscle id')
+        ax[1].set_title('Correlation Matrix')
+        ax[1].axis('equal')
+
+        # draw model
+        self.model.draw_model(q, False, ax[2], scale=0.7, text=False)
+
+    def calculate_simple_muscles(self, t):
+        """Construct Z f_m0 <= B for the case of a linear muscle model for a particular
+        time instance.
+
+        Parameters
+        ----------
+
+        t: time
+
+        """
+        # find nearesrt index corresponding to t
+        idx = np.abs(np.array(self.simulation_reporter.t) - t).argmin()
+        t = self.simulation_reporter.t[idx]
+        q = self.simulation_reporter.q[idx]
+        u = self.simulation_reporter.u[idx]
+        tau = self.simulation_reporter.tau[idx]
+        pose = self.model.model_parameters(q=q, u=u)
+        n = self.model.nd
+
+        # calculate required variables
+        R = to_np_mat(self.model.R.subs(pose))
+        RBarT = np.asmatrix(np.linalg.pinv(R.T))
+        # reduce to independent columns to avoid singularities (proposition 3)
+        NR = nullspace(R.transpose())
+        fm_par = np.asmatrix(-RBarT * tau.reshape((n, 1)))
+        Fmax = to_np_mat(self.model.Fmax)
+
+        Z, B = construct_muscle_space_inequality(NR, fm_par, Fmax)
+
+        return q, Z, B, NR, fm_par
+
+    def generate_solutions(self, A, b, NR, fm_par):
+        """Sample the solution space that satisfy A x <= b.
+
+        Parameters
+        ----------
+
+        A: matrix A
+
+        b: column vector
+
+        NR: moment arm nullspace
+
+        fm_par: particular solution
+
+        Returns
+        -------
+
+        muscle forces: a set of solutions that satisfy the problem
+
+        """
+        feasible_set = []
+        fm0_set = convex_bounded_vertex_enumeration(np.array(A),
+                                                    np.array(b).flatten(), 0)
+        n = fm0_set.shape[0]
+        for i in range(0, n):
+            fm = fm_par + NR * np.matrix(fm0_set[i, :]).reshape(-1, 1)
+            feasible_set.append(fm)
+
+        return np.array(feasible_set).reshape(n, -1)
+
+
+def test_feasible_set(model):
+    feasible_set = FeasibleMuscleSetAnalysis(model)
+    n = model.nd
+    m = model.md
+    feasible_set.record(1,
+                        np.random.random((m, 1)),
+                        np.random.random((m, m)),
+                        np.random.random((n, 1)))
+    fig, ax = plt.subplots(2, 3, figsize=(10, 10))
+    feasible_set.visualize_simple_muscle(1, ax[0])
+    feasible_set.visualize_simple_muscle(1, ax[1])
+    plt.show()