original_docstring: Runs the plant with an initial condition and goal.
  
      Args:
        plant: plant object to use.
        end_goal: end_goal state.
        controller: AngularSystem object to get K from, or None if we should
            use plant.
        observer: AngularSystem object to use for the observer, or None if we
            should use the actual state.
        duration: float, time in seconds to run the simulation for.
        kick_time: float, time in seconds to kick the robot.
        kick_magnitude: float, disturbance in volts to apply.
        max_velocity: float, The maximum velocity for the profile.
        max_acceleration: float, The maximum acceleration for the profile.
      
docstring: Runs the plant with an initial condition and goal.
docstring_tokens: Runs the plant with an initial condition and goal .
code: def RunTest(plant,
              end_goal,
              controller,
              observer=None,
              duration=1.0,
              use_profile=True,
              kick_time=0.5,
              kick_magnitude=0.0,
              max_velocity=10.0,
              max_acceleration=70.0):
      """Runs the plant with an initial condition and goal.
  
      Args:
        plant: plant object to use.
        end_goal: end_goal state.
        controller: AngularSystem object to get K from, or None if we should
            use plant.
        observer: AngularSystem object to use for the observer, or None if we
            should use the actual state.
        duration: float, time in seconds to run the simulation for.
        kick_time: float, time in seconds to kick the robot.
        kick_magnitude: float, disturbance in volts to apply.
        max_velocity: float, The maximum velocity for the profile.
        max_acceleration: float, The maximum acceleration for the profile.
      """
      t_plot = []
      x_plot = []
      v_plot = []
      a_plot = []
      x_goal_plot = []
      v_goal_plot = []
      x_hat_plot = []
      u_plot = []
      offset_plot = []
  
      if controller is None:
          controller = plant
  
      vbat = 12.0
  
      goal = numpy.concatenate((plant.X, numpy.matrix(numpy.zeros((1, 1)))),
                               axis=0)
  
      profile = TrapezoidProfile(plant.dt)
      profile.set_maximum_acceleration(max_acceleration)
      profile.set_maximum_velocity(max_velocity)
      profile.SetGoal(goal[0, 0])
  
      U_last = numpy.matrix(numpy.zeros((1, 1)))
      iterations = int(duration / plant.dt)
      for i in range(iterations):
          t = i * plant.dt
          observer.Y = plant.Y
          observer.CorrectObserver(U_last)
  
          offset_plot.append(observer.X_hat[2, 0])
          x_hat_plot.append(observer.X_hat[0, 0])
  
          next_goal = numpy.concatenate(
              (profile.Update(end_goal[0, 0], end_goal[1, 0]),
               numpy.matrix(numpy.zeros((1, 1)))),
              axis=0)
  
          ff_U = controller.Kff * (next_goal - observer.A * goal)
  
          if use_profile:
              U_uncapped = controller.K * (goal - observer.X_hat) + ff_U
              x_goal_plot.append(goal[0, 0])
              v_goal_plot.append(goal[1, 0])
          else:
              U_uncapped = controller.K * (end_goal - observer.X_hat)
              x_goal_plot.append(end_goal[0, 0])
              v_goal_plot.append(end_goal[1, 0])
  
          U = U_uncapped.copy()
          U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
          x_plot.append(plant.X[0, 0])
  
          if v_plot:
              last_v = v_plot[-1]
          else:
              last_v = 0
  
          v_plot.append(plant.X[1, 0])
          a_plot.append((v_plot[-1] - last_v) / plant.dt)
  
          u_offset = 0.0
          if t >= kick_time:
              u_offset = kick_magnitude
          plant.Update(U + u_offset)
  
          observer.PredictObserver(U)
  
          t_plot.append(t)
          u_plot.append(U[0, 0])
  
          ff_U -= U_uncapped - U
          goal = controller.A * goal + controller.B * ff_U
  
          if U[0, 0] != U_uncapped[0, 0]:
              profile.MoveCurrentState(
                  numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
  
      glog.debug('Time: %f', t_plot[-1])
      glog.debug('goal_error %s', repr(end_goal - goal))
      glog.debug('error %s', repr(observer.X_hat - end_goal))
  
      pylab.subplot(3, 1, 1)
      pylab.plot(t_plot, x_plot, label='x')
      pylab.plot(t_plot, x_hat_plot, label='x_hat')
      pylab.plot(t_plot, x_goal_plot, label='x_goal')
      pylab.legend()
  
      pylab.subplot(3, 1, 2)
      pylab.plot(t_plot, u_plot, label='u')
      pylab.plot(t_plot, offset_plot, label='voltage_offset')
      pylab.legend()
  
      pylab.subplot(3, 1, 3)
      pylab.plot(t_plot, a_plot, label='a')
      pylab.legend()
  
      pylab.show()
code_tokens: def RunTest ( plant , end_goal , controller , observer = None , duration = 1.0 , use_profile = True , kick_time = 0.5 , kick_magnitude = 0.0 , max_velocity = 10.0 , max_acceleration = 70.0 ) : t_plot = [ ] x_plot = [ ] v_plot = [ ] a_plot = [ ] x_goal_plot = [ ] v_goal_plot = [ ] x_hat_plot = [ ] u_plot = [ ] offset_plot = [ ] if controller is None : controller = plant vbat = 12.0 goal = numpy . concatenate ( ( plant . X , numpy . matrix ( numpy . zeros ( ( 1 , 1 ) ) ) ) , axis = 0 ) profile = TrapezoidProfile ( plant . dt ) profile . set_maximum_acceleration ( max_acceleration ) profile . set_maximum_velocity ( max_velocity ) profile . SetGoal ( goal [ 0 , 0 ] ) U_last = numpy . matrix ( numpy . zeros ( ( 1 , 1 ) ) ) iterations = int ( duration / plant . dt ) for i in range ( iterations ) : t = i * plant . dt observer . Y = plant . Y observer . CorrectObserver ( U_last ) offset_plot . append ( observer . X_hat [ 2 , 0 ] ) x_hat_plot . append ( observer . X_hat [ 0 , 0 ] ) next_goal = numpy . concatenate ( ( profile . Update ( end_goal [ 0 , 0 ] , end_goal [ 1 , 0 ] ) , numpy . matrix ( numpy . zeros ( ( 1 , 1 ) ) ) ) , axis = 0 ) ff_U = controller . Kff * ( next_goal - observer . A * goal ) if use_profile : U_uncapped = controller . K * ( goal - observer . X_hat ) + ff_U x_goal_plot . append ( goal [ 0 , 0 ] ) v_goal_plot . append ( goal [ 1 , 0 ] ) else : U_uncapped = controller . K * ( end_goal - observer . X_hat ) x_goal_plot . append ( end_goal [ 0 , 0 ] ) v_goal_plot . append ( end_goal [ 1 , 0 ] ) U = U_uncapped . copy ( ) U [ 0 , 0 ] = numpy . clip ( U [ 0 , 0 ] , - vbat , vbat ) x_plot . append ( plant . X [ 0 , 0 ] ) if v_plot : last_v = v_plot [ - 1 ] else : last_v = 0 v_plot . append ( plant . X [ 1 , 0 ] ) a_plot . append ( ( v_plot [ - 1 ] - last_v ) / plant . dt ) u_offset = 0.0 if t >= kick_time : u_offset = kick_magnitude plant . Update ( U + u_offset ) observer . PredictObserver ( U ) t_plot . append ( t ) u_plot . append ( U [ 0 , 0 ] ) ff_U -= U_uncapped - U goal = controller . A * goal + controller . B * ff_U if U [ 0 , 0 ] != U_uncapped [ 0 , 0 ] : profile . MoveCurrentState ( numpy . matrix ( [ [ goal [ 0 , 0 ] ] , [ goal [ 1 , 0 ] ] ] ) ) glog . debug ( 'Time: %f' , t_plot [ - 1 ] ) glog . debug ( 'goal_error %s' , repr ( end_goal - goal ) ) glog . debug ( 'error %s' , repr ( observer . X_hat - end_goal ) ) pylab . subplot ( 3 , 1 , 1 ) pylab . plot ( t_plot , x_plot , label = 'x' ) pylab . plot ( t_plot , x_hat_plot , label = 'x_hat' ) pylab . plot ( t_plot , x_goal_plot , label = 'x_goal' ) pylab . legend ( ) pylab . subplot ( 3 , 1 , 2 ) pylab . plot ( t_plot , u_plot , label = 'u' ) pylab . plot ( t_plot , offset_plot , label = 'voltage_offset' ) pylab . legend ( ) pylab . subplot ( 3 , 1 , 3 ) pylab . plot ( t_plot , a_plot , label = 'a' ) pylab . legend ( ) pylab . show ( )
short_docstring: Runs the plant with an initial condition and goal.
short_docstring_tokens: Runs the plant with an initial condition and goal .
comment: """Runs the plant with an initial condition and goal.
  
      Args:
        plant: plant object to use.
        end_goal: end_goal state.
        controller: AngularSystem object to get K from, or None if we should
            use plant.
        observer: AngularSystem object to use for the observer, or None if we
            should use the actual state.
        duration: float, time in seconds to run the simulation for.
        kick_time: float, time in seconds to kick the robot.
        kick_magnitude: float, disturbance in volts to apply.
        max_velocity: float, The maximum velocity for the profile.
        max_acceleration: float, The maximum acceleration for the profile.
      """