Discretization
This page gives details on the methods for evaluating discretized dynamics, as well as instructions on how to define a custom integration method.
Model Discretization
With a model defined, we can compute the discrete dynamics and discrete dynamics Jacobians for an Implicit integration rule with the following methods
TrajectoryOptimization.discrete_dynamics — FunctionCompute the discretized dynamics of model using implicit integration scheme Q<:QuadratureRule.
Methods:
x′ = discrete_dynamics(model, model, z) # uses RK3 as the default integration scheme
x′ = discrete_dynamics(Q, model, x, u, t, dt)
x′ = discrete_dynamics(Q, model, z::KnotPoint)The default integration scheme is stored in TrajectoryOptimization.DEFAULT_Q
Missing docstring for discrete_jacobian. Check Documenter's build log for details.
Integration Schemes
TrajectoryOptimization.jl has already defined a handful of integration schemes for computing discrete dynamics. The integration schemes are specified as abstract types, so that methods can efficiently dispatch based on the integration scheme selected. Here is the current set of implemented types:
TrajectoryOptimization.QuadratureRule — TypeIntegration rule for approximating the continuous integrals for the equations of motion
TrajectoryOptimization.Implicit — TypeIntegration rules of the form x′ = f(x,u), where x′ is the next state
TrajectoryOptimization.RK3 — TypeSecond-order Runge-Kutta method with zero-order-old on the controls
TrajectoryOptimization.Explicit — TypeIntegration rules of the form x′ = f(x,u,x′,u′), where x′,u′ are the states and controls at the next time step.
TrajectoryOptimization.HermiteSimpson — TypeThird-order Runge-Kutta method with first-order-hold on the controls
Defining a New Integration Scheme
Implicit Methods
Implicit integration schemes are understandably simpler, since the output is not a function of itself, as is the case with explicit jschemes. As such, as a minimum, the user only needs to define the following method for a new rule MyQ:
x′ = discrete_dynamics(::Type{MyQ}, model::AbstractModel, x, u, dt)Explicit Methods
Explicit integration schemes are specified with a DynamicsConstraint. These methods are most efficiently computed when the entire trajectory is considered at once, thereby avoiding duplicate function evaluations. As a result, the user must define methods that deal with the entire trajectory at once:
evaluate!(vals::Vector{<:AbstractVector}, con::DynamicsConstraint{MyQ},
Z::Traj, inds=1:length(Z)-1)Here vals is a Vector of Static Vectors, where the result of the calculation will be stored. con is a DynamicsConstraint that specifies the integration scheme, Z is the trajectory, and inds are the knotpoints where the constraint is applied (which should always be 1:N-1 if you have a single model for the entire trajectory). The method should compute
vals[k] = x[k+1] - f(x[k],u[k],x[k+1],u[k+1])which is the amount of dynamic infeasibility between knotpoints. The method should obviously loop over the entire trajectory (see implementation for HermiteSimpson).
Integrating Cost Functions
Some methods, such as DIRCOL, apply the integration scheme to the cost function, as well. This can be done for a new integration rule by defining the following methods:
cost(obj::Objective, dyn_con::DynamicsConstraint{MyQ}, Z::Traj)
cost_gradient!(E::CostExpansion, obj::Objective, dyn_con::DynamicsConstraint{MyQ}, Z::Traj)