def trapezoidal_command(CurrTime, Distance, Vmax, Accel, StartTime=0.):
    """
    Function to generate a trapezoidal velocity command
    
    Arguments:
      CurrTime : The current timestep or an array of times
      Distance : The distance to travel over
      Vmax: The maximum velocity to reach
      Accel: The acceleration, assumed to be symmetric
      StartTime : The time that the command should StartTime
      
    Returns :
      The command at the current timestep or an array representing the command
      over the times given (if CurrTime was an array)
    """
    
    t1 = StartTime + Vmax / Accel
    t2 = Distance / Vmax + StartTime
    t3 = t2 + Vmax / Accel
    
    #pdb.set_trace()

    # We'll create the command by just superimposing 4 ramps starting at 
    # StartTime, t1, t2, and t3
    trapezoidal = (Accel * (CurrTime - StartTime) * (CurrTime - StartTime >= 0) +
                   -(Accel * (CurrTime - t1) * (CurrTime - t1 >= 0)) +
                   -(Accel * (CurrTime - t2) * (CurrTime - t2 >= 0)) +
                   (Accel * (CurrTime - t3) * (CurrTime - t3 >= 0)))
    
    return trapezoidal