double_support.py 文件源码

def __init__(self, nb_steps, duration, omega2, state_estimator, com_target,
                 stance, tube_radius=0.02, tube_margin=0.01):
        start_com = state_estimator.com
        start_comd = state_estimator.comd
        tube = COMTube(
            start_com, com_target.p, stance, tube_radius, tube_margin)
        dt = duration / nb_steps
        I = eye(3)
        A = asarray(bmat([[I, dt * I], [zeros((3, 3)), I]]))
        B = asarray(bmat([[.5 * dt ** 2 * I], [dt * I]]))
        x_init = hstack([start_com, start_comd])
        x_goal = hstack([com_target.p, com_target.pd])
        C1, e1 = tube.primal_hrep
        D2, e2 = tube.dual_hrep
        C1 = hstack([C1, zeros(C1.shape)])
        C2 = zeros((D2.shape[0], A.shape[1]))
        D1 = zeros((C1.shape[0], B.shape[1]))
        C = vstack([C1, C2])
        D = vstack([D1, D2])
        e = hstack([e1, e2])
        lmpc = LinearPredictiveControl(
            A, B, C, D, e, x_init, x_goal, nb_steps, wxt=1., wxc=1e-1, wu=1e-1)
        lmpc.build()
        try:
            lmpc.solve()
            U = lmpc.U
            P = lmpc.X[:-1, 0:3]
            V = lmpc.X[:-1, 3:6]
            Z = P + (gravity - U) / omega2
            preview = ZMPPreviewBuffer([stance])
            preview.update(P, V, Z, [dt] * nb_steps, omega2)
        except ValueError as e:
            print "%s error:" % type(self).__name__, e
            preview = None
        self.lmpc = lmpc
        self.preview = preview
        self.tube = tube