Class OCP

java.lang.Object
org.wpilib.math.optimization.Problem
org.wpilib.math.optimization.OCP
All Implemented Interfaces:
AutoCloseable
public class OCP extends Problem
This class allows the user to pose and solve a constrained optimal control problem (OCP) in a variety of ways.

The system is transcripted by one of three methods (direct transcription, direct collocation, or single-shooting) and additional constraints can be added.

In direct transcription, each state is a decision variable constrained to the integrated dynamics of the previous state. In direct collocation, the trajectory is modeled as a series of cubic polynomials where the centerpoint slope is constrained. In single-shooting, states depend explicitly as a function of all previous states and all previous inputs.

Explicit ODEs are integrated using RK4.

For explicit ODEs, the function must be in the form dx/dt = f(t, x, u). For discrete state transition functions, the function must be in the form xₖ₊₁ = f(t, xₖ, uₖ).

Direct collocation requires an explicit ODE. Direct transcription and single-shooting can use either an ODE or state transition function.

https://underactuated.mit.edu/trajopt.html goes into more detail on each transcription method.

  • Constructor Details

    • OCP

      public OCP(int numStates, int numInputs, double dt, int numSteps, BiFunction<VariableMatrix, VariableMatrix, VariableMatrix> dynamics, DynamicsType dynamicsType, TimestepMethod timestepMethod, TranscriptionMethod transcriptionMethod)
      Builds an optimization problem using a system evolution function (explicit ODE or discrete state transition function).
      Parameters:
      numStates - The number of system states.
      numInputs - The number of system inputs.
      dt - The timestep for fixed-step integration.
      numSteps - The number of control points.
      dynamics - Function representing an explicit or implicit ODE, or a discrete state transition function.
      • Explicit: dx/dt = f(x, u, *)
      • Implicit: f([x dx/dt]', u, *) = 0
      • State transition: xₖ₊₁ = f(xₖ, uₖ)
      dynamicsType - The type of system evolution function.
      timestepMethod - The timestep method.
      transcriptionMethod - The transcription method.

    • OCP

      public OCP(int numStates, int numInputs, double dt, int numSteps, DynamicsFunction dynamics, DynamicsType dynamicsType, TimestepMethod timestepMethod, TranscriptionMethod transcriptionMethod)
      Builds an optimization problem using a system evolution function (explicit ODE or discrete state transition function).
      Parameters:
      numStates - The number of system states.
      numInputs - The number of system inputs.
      dt - The timestep for fixed-step integration.
      numSteps - The number of control points.
      dynamics - Function representing an explicit or implicit ODE, or a discrete state transition function.
      • Explicit: dx/dt = f(t, x, u, *)
      • Implicit: f(t, [x dx/dt]', u, *) = 0
      • State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt)
      dynamicsType - The type of system evolution function.
      timestepMethod - The timestep method.
      transcriptionMethod - The transcription method.

  • Method Details

    • constrainInitialState

      public void constrainInitialState(double initialState)
      Constrains the initial state.
      Parameters:
      initialState - the initial state to constrain to.

    • constrainInitialState

      public void constrainInitialState(Variable initialState)
      Constrains the initial state.
      Parameters:
      initialState - the initial state to constrain to.

    • constrainInitialState

      public void constrainInitialState(org.ejml.simple.SimpleMatrix initialState)
      Constrains the initial state.
      Parameters:
      initialState - the initial state to constrain to.

    • constrainInitialState

      public void constrainInitialState(VariableMatrix initialState)
      Constrains the initial state.
      Parameters:
      initialState - the initial state to constrain to.

    • constrainInitialState

      public void constrainInitialState(VariableBlock initialState)
      Constrains the initial state.
      Parameters:
      initialState - the initial state to constrain to.

    • constrainFinalState

      public void constrainFinalState(double finalState)
      Constrains the final state.
      Parameters:
      finalState - the final state to constrain to.

    • constrainFinalState

      public void constrainFinalState(Variable finalState)
      Constrains the final state.
      Parameters:
      finalState - the final state to constrain to.

    • constrainFinalState

      public void constrainFinalState(org.ejml.simple.SimpleMatrix finalState)
      Constrains the final state.
      Parameters:
      finalState - the final state to constrain to.

    • constrainFinalState

      public void constrainFinalState(VariableMatrix finalState)
      Constrains the final state.
      Parameters:
      finalState - the final state to constrain to.

    • constrainFinalState

      public void constrainFinalState(VariableBlock finalState)
      Constrains the final state.
      Parameters:
      finalState - the final state to constrain to.

    • forEachStep

      public void forEachStep(BiConsumer<VariableMatrix, VariableMatrix> callback)
      Sets the constraint evaluation function. This function is called `numSteps+1` times, with the corresponding state and input VariableMatrices.
      Parameters:
      callback - The callback f(x, u) where x is the state and u is the input vector.

    • forEachStep

      public void forEachStep(ConstraintEvaluationFunction callback)
      Sets the constraint evaluation function. This function is called `numSteps+1` times, with the corresponding state and input VariableMatrices.
      Parameters:
      callback - The callback f(t, x, u, dt) where t is time, x is the state vector, u is the input vector, and dt is the timestep duration.

    • setLowerInputBound

      public void setLowerInputBound(double lowerBound)
      Sets a lower bound on the input.
      Parameters:
      lowerBound - The lower bound that inputs must always be above. Must be shaped (numInputs)x1.

    • setLowerInputBound

      public void setLowerInputBound(Variable lowerBound)
      Sets a lower bound on the input.
      Parameters:
      lowerBound - The lower bound that inputs must always be above. Must be shaped (numInputs)x1.

    • setLowerInputBound

      public void setLowerInputBound(org.ejml.simple.SimpleMatrix lowerBound)
      Sets a lower bound on the input.
      Parameters:
      lowerBound - The lower bound that inputs must always be above. Must be shaped (numInputs)x1.

    • setLowerInputBound

      public void setLowerInputBound(VariableMatrix lowerBound)
      Sets a lower bound on the input.
      Parameters:
      lowerBound - The lower bound that inputs must always be above. Must be shaped (numInputs)x1.

    • setLowerInputBound

      public void setLowerInputBound(VariableBlock lowerBound)
      Sets a lower bound on the input.
      Parameters:
      lowerBound - The lower bound that inputs must always be above. Must be shaped (numInputs)x1.

    • setUpperInputBound

      public void setUpperInputBound(double upperBound)
      Sets an upper bound on the input.
      Parameters:
      upperBound - The upper bound that inputs must always be below. Must be shaped (numInputs)x1.

    • setUpperInputBound

      public void setUpperInputBound(Variable upperBound)
      Sets an upper bound on the input.
      Parameters:
      upperBound - The upper bound that inputs must always be below. Must be shaped (numInputs)x1.

    • setUpperInputBound

      public void setUpperInputBound(org.ejml.simple.SimpleMatrix upperBound)
      Sets an upper bound on the input.
      Parameters:
      upperBound - The upper bound that inputs must always be below. Must be shaped (numInputs)x1.

    • setUpperInputBound

      public void setUpperInputBound(VariableMatrix upperBound)
      Sets an upper bound on the input.
      Parameters:
      upperBound - The upper bound that inputs must always be below. Must be shaped (numInputs)x1.

    • setUpperInputBound

      public void setUpperInputBound(VariableBlock upperBound)
      Sets an upper bound on the input.
      Parameters:
      upperBound - The upper bound that inputs must always be below. Must be shaped (numInputs)x1.

    • setMinTimestep

      public void setMinTimestep(double minTimestep)
      Sets a lower bound on the timestep.
      Parameters:
      minTimestep - The minimum timestep in seconds.

    • setMaxTimestep

      public void setMaxTimestep(double maxTimestep)
      Sets an upper bound on the timestep.
      Parameters:
      maxTimestep - The maximum timestep in seconds.

    • X

      public VariableMatrix X()
      Gets the state variables. After the problem is solved, this will contain the optimized trajectory.

      Shaped (numStates)x(numSteps+1).

      Returns:
      The state variable matrix.

    • U

      public VariableMatrix U()
      Gets the input variables. After the problem is solved, this will contain the inputs corresponding to the optimized trajectory.

      Shaped (numInputs)x(numSteps+1), although the last input step is unused in the trajectory.

      Returns:
      The input variable matrix.

    • dt

      public VariableMatrix dt()
      Gets the timestep variables. After the problem is solved, this will contain the timesteps corresponding to the optimized trajectory.

      Shaped 1x(numSteps+1), although the last timestep is unused in the trajectory.

      Returns:
      The timestep variable matrix.

    • initialState

      public VariableMatrix initialState()
      Gets the initial state in the trajectory.
      Returns:
      The initial state of the trajectory.

    • finalState

      public VariableMatrix finalState()
      Gets the final state in the trajectory.
      Returns:
      The final state of the trajectory.