preview.py 文件源码

def discretize(self, nb_steps):
        """
        Discretize the remaining preview with a uniform timestep.

        Parameter
        ---------
        nb_steps : integer
            Number of discretization time steps.

        Returns
        -------
        X : array, shape=(N + 1, 9)
            Series of discretized states (com, comd, zmp).
        contacts : list of Contacts
            List of N + 1 contacts corresponding to each step k from 0 to N.
        """
        assert isreal(self.omega2), "Discretization only works on FIP previews"
        X = []
        contacts = []
        input_step = self.cur_step
        com, comd = self.P[0], self.V[0]
        output_timestep = self.rem_duration / nb_steps
        rem_dT = self.rem_dT
        omega2 = self.omega2
        for _ in xrange(nb_steps):
            X.append(hstack([com, comd, self.get_zmp(input_step)]))
            contacts.append(self.get_contact(input_step))
            time_to_fill = output_timestep
            while time_to_fill > 1e-10:
                if rem_dT < 1e-10:
                    input_step += 1
                    rem_dT = self.get_dT(input_step)
                zmp = self.get_zmp(input_step)
                dt = min(time_to_fill, rem_dT)
                com, comd = integrate_fip(com, comd, zmp, dt, omega2)
                time_to_fill -= dt
                rem_dT -= dt
        cp = com + comd / sqrt(omega2) + gravity / omega2
        X.append(hstack([com, comd, cp]))
        contacts.append(contacts[-1])
        return array(X), contacts