(t)
return speed[0:3], speed[3]
Creates an example speed profile based on time for demo purpose.
Creates an example speed profile based on time for demo purpose.
Creates an example speed profile based on time for demo purpose .
def _generate_example_linear_angular_speed(t):
  """Creates an example speed profile based on time for demo purpose."""
  vx = 0.6
  vy = 0.2
  wz = 0.8
  time_points = (0, 5, 10, 15, 20, 25,30)
  speed_points = ((0, 0, 0, 0), (0, 0, 0, wz), (vx, 0, 0, 0), (0, 0, 0, -wz), (0, -vy, 0, 0),
                  (0, 0, 0, 0), (0, 0, 0, wz))

  speed = scipy.interpolate.interp1d(
      time_points,
      speed_points,
      kind="previous",
      fill_value="extrapolate",
      axis=0)(
          t)

  return speed[0:3], speed[3]
def _generate_example_linear_angular_speed ( t ) : vx = 0.6 vy = 0.2 wz = 0.8 time_points = ( 0 , 5 , 10 , 15 , 20 , 25 , 30 ) speed_points = ( ( 0 , 0 , 0 , 0 ) , ( 0 , 0 , 0 , wz ) , ( vx , 0 , 0 , 0 ) , ( 0 , 0 , 0 , - wz ) , ( 0 , - vy , 0 , 0 ) , ( 0 , 0 , 0 , 0 ) , ( 0 , 0 , 0 , wz ) ) speed = scipy . interpolate . interp1d ( time_points , speed_points , kind = "previous" , fill_value = "extrapolate" , axis = 0 ) ( t ) return speed [ 0 : 3 ] , speed [ 3 ]