Add robot_souple_helpers.py dependency

robot_souple_helpers.py

@@ -0,0 +1,191 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+def Heaviside(t):
+    return np.heaviside(t - 1, 1)
+
+def difHeaviside(t):
+    return np.zeros_like(t)
+
+def intHeaviside(t):
+    return np.where(t >= 1, t, 0)
+
+def generateNoiseTemporal(time, tlength, q, vseed):
+    rng = np.random.RandomState(vseed)
+    nstep = len(time)
+    seed = rng.randn(nstep)
+    time = np.array(time)
+    dt = time[:, np.newaxis] - time[np.newaxis, :]
+    Corr = np.exp(-dt**2 / tlength**2)
+    fpert = Corr @ seed
+    famp = np.sum(fpert**2) / nstep
+    fpert = fpert * np.sqrt(q / famp)
+    return fpert
+
+def neb_beam_matrices(nx, dx, E, Ix, rho, S, cy):
+    npt = nx + 1
+    ndof = 2 * npt
+    doflist = np.arange(0, ndof - 3, 2)
+    on1 = np.ones(nx)
+    Ki = np.concatenate((doflist, doflist, doflist, doflist,
+                         doflist + 1, doflist + 1, doflist + 1, doflist + 1,
+                         doflist + 2, doflist + 2, doflist + 2, doflist + 2,
+                         doflist + 3, doflist + 3, doflist + 3, doflist + 3))
+    Kj = np.concatenate((doflist, doflist + 1, doflist + 2, doflist + 3,
+                         doflist, doflist + 1, doflist + 2, doflist + 3,
+                         doflist, doflist + 1, doflist + 2, doflist + 3,
+                         doflist, doflist + 1, doflist + 2, doflist + 3))
+    Kv = E * Ix / (dx**3) * np.concatenate((12 * on1, 6 * dx * on1, -12 * on1, 6 * dx * on1,
+                                            6 * dx * on1, 4 * dx**2 * on1, -6 * dx * on1, 2 * dx**2 * on1,
+                                            -12 * on1, -6 * dx * on1, 12 * on1, -6 * dx * on1,
+                                            6 * dx * on1, 2 * dx**2 * on1, -6 * dx * on1, 4 * dx**2 * on1))
+    Kfull = np.zeros((ndof, ndof))
+    for ii in range(len(Ki)):
+        Kfull[int(Ki[ii]), int(Kj[ii])] += Kv[ii]
+    Mv1 = rho * S * dx / 420 * np.concatenate((156 * on1, 22 * dx * on1, 54 * on1, -13 * dx * on1,
+                                               22 * dx * on1, 4 * dx**2 * on1, 13 * dx * on1, -3 * dx**2 * on1,
+                                               54 * on1, 13 * dx * on1, 156 * on1, -22 * dx * on1,
+                                               -13 * dx * on1, -3 * dx**2 * on1, -22 * dx * on1, 4 * dx**2 * on1))
+    Mfull = np.zeros((ndof, ndof))
+    for ii in range(len(Ki)):
+        Mfull[int(Ki[ii]), int(Kj[ii])] += Mv1[ii]
+    Cv = cy * dx / 420 * np.concatenate((156 * on1, 22 * dx * on1, 54 * on1, -13 * dx * on1,
+                                         22 * dx * on1, 4 * dx**2 * on1, 13 * dx * on1, -3 * dx**2 * on1,
+                                         54 * on1, 13 * dx * on1, 156 * on1, -22 * dx * on1,
+                                         -13 * dx * on1, -3 * dx**2 * on1, -22 * dx * on1, 4 * dx**2 * on1))
+    Cfull = np.zeros((ndof, ndof))
+    for ii in range(len(Ki)):
+        Cfull[int(Ki[ii]), int(Kj[ii])] += Cv[ii]
+    return Kfull, Cfull, Mfull
+
+def newmark1stepMRHS(M, C, K, f, u0, v0, a0, dt, beta, gamma):
+    nrhs = u0.shape[1] if u0.ndim == 2 else 1
+    fatK = K + 1 / (beta * dt**2) * M + gamma / (beta * dt) * C
+    if nrhs > 1:
+        if f.shape[1] == 1:
+            f = np.tile(f, (1, nrhs))
+        b = f + C @ (gamma / (beta * dt) * u0 + (gamma / beta - 1) * v0 + dt / 2 * (gamma / beta - 2) * a0) + \
+            M @ (1 / (beta * dt**2) * u0 + 1 / (beta * dt) * v0 + (1 / (2 * beta) - 1) * a0)
+        u = np.zeros_like(b)
+        for jj in range(nrhs):
+            u[:, jj] = np.linalg.solve(fatK, b[:, jj])
+    else:
+        b = f + C @ (gamma / (beta * dt) * u0 + (gamma / beta - 1) * v0 + dt / 2 * (gamma / beta - 2) * a0) + \
+            M @ (1 / (beta * dt**2) * u0 + 1 / (beta * dt) * v0 + (1 / (2 * beta) - 1) * a0)
+        u = np.linalg.solve(fatK, b)
+    a = 1 / (beta * dt**2) * (u - u0 - dt * v0 - dt**2 / 2 * (1 - 2 * beta) * a0)
+    v = v0 + dt * ((1 - gamma) * a0 + gamma * a)
+    return u, v, a
+
+def Bconstruct(M, C, K, nA, dt, beta, gamma):
+    nx = M.shape[0]
+    B = np.zeros((3 * nx, nx))
+    for j in range(nx):
+        u0 = np.zeros(nx)
+        v0 = np.zeros(nx)
+        a0 = np.zeros(nx)
+        eta = np.zeros(nx)
+        eta[j] = 1
+        u, v, a = newmark1stepMRHS(M, C, K, eta, u0, v0, a0, dt, beta, gamma)
+        B[:, j] = np.concatenate((u, v, a))
+    return B
+
+def Fconstruct(M, C, K, dt, beta, gamma):
+    nx = M.shape[0]
+    M1 = np.eye(nx)
+    M2 = np.zeros((nx, nx))
+    u0 = np.hstack((M1, M2, M2))
+    v0 = np.hstack((M2, M1, M2))
+    a0 = np.hstack((M2, M2, M1))
+    f = np.zeros(nx)
+    u, v, a = newmark1stepMRHS(M, C, K, f, u0, v0, a0, dt, beta, gamma)
+    F = np.vstack((u, v, a))
+    return F
+
+def Newmark(M, C, K, f, u0, v0, a0, dt, beta, gamma, fatK1=None):
+    ndof, ntime = f.shape
+    u = np.zeros((ndof, ntime))
+    v = np.zeros((ndof, ntime))
+    a = np.zeros((ndof, ntime))
+    up = u0.copy()
+    vp = v0.copy()
+    ap = a0.copy()
+    fatK = K + 1 / (beta * dt**2) * M + gamma / (beta * dt) * C
+    invert = fatK1 is not None
+    if not invert:
+        if 2 * ntime > ndof:
+            fatK1 = np.linalg.inv(fatK)
+            invert = True
+    u[:, 0] = u0
+    v[:, 0] = v0
+    a[:, 0] = a0
+    for i in range(1, ntime):
+        b = f[:, i] + C @ (gamma / (beta * dt) * up + (gamma / beta - 1) * vp + dt / 2 * (gamma / beta - 2) * ap) + \
+            M @ (1 / (beta * dt**2) * up + 1 / (beta * dt) * vp + (1 / (2 * beta) - 1) * ap)
+        if invert:
+            u[:, i] = fatK1 @ b
+        else:
+            u[:, i] = np.linalg.solve(fatK, b)
+        a[:, i] = 1 / (beta * dt**2) * (u[:, i] - up - dt * vp - dt**2 / 2 * (1 - 2 * beta) * ap)
+        v[:, i] = vp + dt * ((1 - gamma) * ap + gamma * a[:, i])
+        up = u[:, i]
+        vp = v[:, i]
+        ap = a[:, i]
+    return u, v, a
+
+def forthodir(m, c, k, co, u0, v0, niter, nopt, stagEps, delta, beta, gamma, mu, picture=False, indices=None, action=None):
+    ndof = len(u0)
+    if indices is None:
+        indices = np.arange(ndof)
+    if action is None:
+        action = np.arange(ndof)
+    nact = len(action)
+    fop = np.zeros((ndof, niter))
+    ress = np.zeros(nopt)
+    Delta = np.zeros(nopt)
+    dirs = np.zeros((nact * niter, nopt))
+    Ads = np.zeros((nact * niter, nopt))
+    diff = np.zeros((ndof, niter))
+    dirdir = np.zeros((ndof, niter))
+    ai0 = np.linalg.solve(m, - (c @ v0 + k @ u0))
+    ui, vi, ai = Newmark(m, c, k, np.zeros((ndof, niter)), u0, v0, ai0, delta, beta, gamma)
+    diff[indices, :] = co[indices, :] - ui[indices, :]
+    laa0 = np.linalg.solve(m, diff[:, -1])
+    lau, lav, laa = Newmark(m, c, k, diff[:, ::-1], np.zeros(ndof), np.zeros(ndof), laa0, delta, beta, gamma)
+    laulau = lau[action, ::-1]
+    rhs = laulau.flatten()
+    resv = rhs.copy()
+    res0 = np.linalg.norm(resv)
+    if res0 < stagEps:
+        print('zero seems to be the optimal solution')
+        return fop, np.array([])
+    n_converged = 0
+    for iter_ in range(nopt):
+        dirr = resv.copy()
+        dirdir[action, :] = dirr.reshape((nact, niter))
+        ai0 = np.linalg.solve(m, dirdir[:, 0])
+        ui, vi, ai = Newmark(m, c, k, dirdir, np.zeros(ndof), np.zeros(ndof), ai0, delta, beta, gamma)
+        diff[indices, :] = -ui[indices, :]
+        laa0 = np.linalg.solve(m, diff[:, -1])
+        lau, lav, laa = Newmark(m, c, k, diff[:, ::-1], np.zeros(ndof), np.zeros(ndof), laa0, delta, beta, gamma)
+        laulau = lau[action, ::-1]
+        Ad = mu * dirr - laulau.flatten()
+        for j in range(iter_):
+            phiij = Ads[:, j] @ Ad
+            betaij = phiij / Delta[j]
+            dirr -= betaij * dirs[:, j]
+            Ad -= betaij * Ads[:, j]
+        dirs[:, iter_] = dirr
+        Ads[:, iter_] = Ad
+        Delta[iter_] = Ad @ Ad
+        gammai = Ad @ resv
+        alphai = gammai / Delta[iter_]
+        dirdir[action, :] = dirr.reshape((nact, niter))
+        fop += alphai * dirdir
+        resv -= alphai * Ad
+        ress[iter_] = np.linalg.norm(resv)
+        n_converged = iter_ + 1
+        if ress[iter_] / res0 < stagEps:
+            break
+    ress = ress[:n_converged]
+    return fop, ress