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
评论列表
文章目录