机器人奇妙小工具：一种在线插值

1
itp_state = ItpState()
2
itp_state.init(v_max=v_max, a_max=a_max, j_max=j_max, fps=fps)
3
# config
4
itp_state = ItpState()
5
itp_state.init(v_max=v_max, a_max=a_max, j_max=j_max, fps=fps)
6

7
# set initial state
8
itp_state.init(x0=x0, v0=v0)
9
for i, (x, v, t) in enumerate(zip(x_samples, v_samples, t_samples)):
10
  points_needed = int((t - last_itpltn_t) * fps)
11
  res += itp_state.interpolate(
12
    arr(x), arr(v), points_needed, first_delta_t=(last_itpltn_t - t)
13
  )