(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 * robot_sim.MPC_VELOCITY_MULTIPLIER
  vy = 0.2 * robot_sim.MPC_VELOCITY_MULTIPLIER
  wz = 0.8 * robot_sim.MPC_VELOCITY_MULTIPLIER
  
  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 * robot_sim . MPC_VELOCITY_MULTIPLIER vy = 0.2 * robot_sim . MPC_VELOCITY_MULTIPLIER wz = 0.8 * robot_sim . MPC_VELOCITY_MULTIPLIER 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 ]