$\newcommand{\deriv}[2][]{\dfrac{\partial #1}{\partial #2}} \newcommand{\tderiv}[2][]{\tfrac{\partial #1}{\partial #2}} \newcommand{\fderiv}[2][]{\dfrac{d #1}{d #2}} \newcommand{\derivII}[3][]{\dfrac{\partial^2 #1}{\partial #2 \partial #3}} \newcommand{\tderivII}[3][]{\tfrac{\partial^2 #1}{\partial #2 \partial #3}} \newcommand{\derivIII}[4][]{\dfrac{\partial^3 #1}{\partial #2 \partial #3 \partial #4}} \newcommand{\tderivIII}[4][]{\tfrac{\partial^3 #1}{\partial #2 \partial #3 \partial #4}} \newcommand{\derivIV}[5][]{\dfrac{\partial^4 #1}{\partial #2 \partial #3 \partial #4 \partial #5}} \newcommand{\half}{\tfrac{1}{2}} % Macro for 1/2 \newcommand{\ud}{\mathrm{d}} \newcommand{\op}{\circ} \newcommand{\dq}{\dot{q}} \newcommand{\ddq}{\ddot{q}} \newcommand{\dt}{\Delta t}$

`DSystem` - Discrete System wrapper for Midpoint Variational Integrators¶

DSystem Objects

DSystem objects represent a MidpointVI variational integrators as first order discrete systems of the form $X(k+1) = f(X(k), U(k), k)$. This representation is used by DOptimizer for discrete trajectory optimization. DSystem also provides methods for automatically calculating linearizations and feedback controllers along trajectories.

The discrete state consists of the variational integrator’s full configuration, the dynamic momentum, and the kinematic velocity:

\[\begin{split}X(k) = \begin{bmatrix} q_d(k) \\ q_k(k) \\ p(k) \\ v_k(k) \end{bmatrix}\end{split}\]

The configuration and momentum are the same as in MidpointVI. The kinematic velocity is calculated as:

\[v_k(k) = \frac{q_k(k) - q_k(k-1)}{t(k) - t(k-1)}\]

The discrete input consists of the variational integrator’s force inputs and the future state of the kinematic configurations:

\[\begin{split}U(k) = \begin{bmatrix} u(k) \\ \rho(k) \end{bmatrix}\end{split}\]

where the kinematic inputs are denoted by $\rho$ thoughout this code (i.e, $q_k(k+1) = \rho(k)$). Additionally, the state input U is always capitalized to distinguish it from the force input u which is always lower case.

DSystem provides methods for converting between trajectories for the discrete system and trajectories for the variational integrator.

Examples

pend-on-cart-optimization.py

DSystem Objects ¶

class trep.discopt.DSystem(varint, t)¶

Parameters:	varint (`MidpointVI` instance) – The variational integrator being represented. t (numpy array of floats, shape (N)) – An array of times

Create a discrete system wrapper for the variational integrator varint and the time t. The time t is the array $t(k)$ that maps a discrete time index to a time. It should have the same length N as trajectories used with the system.

DSystem.nX¶

Number of states to the discrete system.

int

DSystem.nU¶

Number of inputs to the discrete system.

int

DSystem.varint¶

The variational integrator wrapped by this instance.

MidpointVI

DSystem.system¶

The mechanical system modeled by the variational integrator.

System

DSystem.time¶

The time of the discrete steps.

numpy array, shape (N)

trep.discopt.xk¶

Current state of the system.

numpy array, shape (nX)

trep.discopt.uk¶

Current input of the system.

numpy array, shape (nU)

trep.discopt.k¶

Current discrete time of the system.

int

DSystem.kf()¶

Return type:	int

Return the last available state that the system can be set to. This is one less than len(self.time).

State and Trajectory Manipulation ¶

DSystem.build_state([Q=None, p=None, v=None])¶

Parameters:	Q (numpy array, shape (nQ)) – A configuration vector p (numpy array, shape (nd)) – A momentum vector v (numpy array, shape (nk)) – A kinematic velocity vector

Build a state vector Xk from components. Unspecified components are set to zero.

DSystem.build_input([u=None, rho=None])¶

Parameters:	u (numpy array, shape (nu)) – An input force vector rho (numpy array, shape (nk)) – A kinematic input vector
Type:	numpy array, shape (nU)

Build an input vector Uk from components. Unspecified components are set to zero.

DSystem.build_trajectory([Q=None, p=None, v=None, u=None, rho=None])¶

Parameters:	Q (numpy array, shape (N, nQ)) – A configuration trajectory p (numpy array, shape (N, nd)) – A momentum trajectory v (numpy array, shape (N, nk)) – A velocity trajectory u (numpy array, shape (N-1, nu)) – An input force trajectory rho (numpy array, shape (N-1, nk)) – A kinematic input trajectory
Return type:	named tuple of (X, U)

Combine component trajectories into a state and input trajectories. The state length is the same as the time base, the input length is one less than the time base. Unspecified components are set to zero:

>>> dsys.build_trajectory() # Create a zero state and input trajectory

DSystem.split_state([X=None])¶

Parameters:	X (numpy array, shape (nX)) – A state vector for the system
Return type:	named tuple of (Q, p, v)

Split a state vector into its configuration, moementum, and kinematic velocity parts. If X is None, returns zero arrays for each component.

DSystem.split_input([U=None])¶

Parameters:	U (numpy array, shape (nU)) – An input vector for the system
Return type:	named tuple of (u, rho)

Split a state input vector U into its force and kinematic input parts, (u, rho). If U is None, returns zero arrays of the appropriate size.

DSystem.split_trajectory([X=None, U=None])¶

Parameters:	X (numpy array, shape (N, nX)) – A state trajectory U (numpy array, shape (N-1, nU)) – An input trajectory
Return type:	named tuple of (Q, p, v, u, rho)

Split the state trajectory (X, U) into its Q, p, v, u, rho components. If X or U are None, the corresponding components are arrays of zero.

DSystem.convert_trajectory(dsys_a, X, U)¶

Parameters:	dsys_a (`DSystem`) – Another discrete system X (numpy array, shape (N, nX)) – A state trajectory for dsys_a U (numpy array, shape (N, nU)) – An input trajectory for dsys_a
Return type:	trajectory for this system, named tuple (X, U)

Convert the trajectory (X, U) for dsys_a into a trajectory for this system. This reorders the trajectory components according to the configuration and input variable names and drops components that aren’t in this system. Variables in this system that are not in dsys_a are replaced with zero.

Note

The returned path may not be a valid trajectory for this system in the sense that $x(k+1) = f(x(k), u(k), k)$. This function only reorders the information.

DSystem.save_state_trajectory(filename[, X=None, U=None])¶

Parameters:	filename (string) – Location to save the trajectory X (numpy array, shape (N, nX)) – A state trajectory U (numpy array, shape (N-1, nU)) – An input trajectory

Save a trajectory to a file. This splits the trajectory with split_trajectory() and saves the results with trep.save_traejctory(). If X or U are not specified, they are replaced with zero arrays.

DSystem.load_state_trajectory(filename)¶

Parameters:	filename (string) – Location of saved trajectory
Return type:	named tuple of (X, U)

Load a trajectory from a file that was stored with save_state_trajectory() or trep.save_trajectory().

If the file does not contain complete information for the system (e.g, it was saved for a different system with different states, or the inputs were not saved), the missing components will be filled with zeros.

Dynamics ¶

DSystem.set(self, xk, uk, k[, xk_hint=None, lambda_hint=None])¶

Set the current state, input, and time of the discrete system.

If xk_hint and lambda_hint are provided, these are used to provide hints to hints to MidpointVI.step(). If the solution is known (for example, if you are calculating the linearization about a known trajectory) this can result in faster performance by reducing the number of root solver iterations in the variational integrator.

DSystem.step(self, uk[, xk_hint=None, lambda_hint=None])¶

Advance the system to the next discrete time using the given input uk.

This is equivalent to calling self.set(self.f(), uk, self.k+1).

If xk_hint and lambda_hint are provided, these are used to provide hints to hints to MidpointVI.step(). If the solution is known (for example, if you are calculating the linearization about a known trajectory) this can result in faster performance by reducing the number of root solver iterations in the variational integrator.

DSystem.f()¶

Return type:	numpy array, shape (nX)

Get the next state of the system, $x(k+1)$.

First Derivatives ¶

DSystem.fdx()¶

Return type:	numpy array, shape (nX, nX)

DSystem.fdu()¶

Return type:	numpy array, shape (nX, nU)

These functions return first derivatives of the system dynamics $f()$ as numpy arrays with the derivatives across the rows.

Second Derivatives ¶

DSystem.fdxdx(z)¶

Parameters:	z (numpy array, shape (nX)) – adjoint vector
Return type:	numpy array, shape (nU, nU)

DSystem.fdxdu(z)¶

Parameters:	z (numpy array, shape (nX)) – adjoint vector
Return type:	numpy array, shape (nX, nU)

DSystem.fdudu(z)¶

Parameters:	z (numpy array, shape (nX)) – adjoint vector
Return type:	numpy array, shape (nU, nU)

These functions return the product of the 1D array z and the second derivative of f. For example:

\[z^T \derivII[f]{u}{u}\]

Linearization and Feedback Controllers ¶

DSystem.linearize_trajectory(X, U)¶

Return type:	named tuple (A, B)

Calculate the linearization of the system dynamics about a trajectory. X and U do not have to be an exact trajectory of the system.

Returns the linearization in a named tuple (A, B).

DSystem.project(bX, bU[, Kproj=None])¶

Rtyple:	named tuple (X, U)

Project bX and bU into a nearby trajectory for the system using a linear feedback law:

X[0] = bX[0]
U[k] = bU[k] - Kproj * (X[k] - bU[k])
X[k+1] = f(X[k], U[k], k)

If no feedback law is specified, one will be created by calc_feedback_controller() along bX and bU. This is typically a bad idea if bX and bU are not very close to an actual trajectory for the system.

Returns the projected trajectory in a named tuple (X, U).

DSystem.dproject(A, B, bdX, bdU, K)¶

Rtyple:	named tuple (dX, dU)

Project bdX and bdU into the tangent trajectory space of the system. A and B are the linearization of the system about the trajectory. K is a stabilizing feedback controller.

Returns the projected tangent trajectory (dX, dU).

DSystem.calc_feedback_controller(X, U[, Q=None, R=None, return_linearization=False])¶

Return type:	K or named tuple (K, A, B)

Calculate a stabilizing feedback controller for the system about a trajectory X and U. The feedback law is calculated by solving the discrete LQR problem for the linearization of the system about X and U.

X and U do not have to be an exact trajectory of the system, but if they are not close, the controller is unlikely to be effective.

If the LQR weights Q and R are not specified, identity matrices are used.

If return_linearization is False, the return value is the feedback control law, K.

If return_linearization is True, the method returns the linearization as well in a named tuple: (K, A, B).

Checking the Derivatives ¶

DSystem.check_fdx(xk, uk, k[, delta=1e-5])¶

DSystem.check_fdu(xk, uk, k[, delta=1e-5])¶

DSystem.check_fdxdx(xk, uk, k[, delta=1e-5])¶

DSystem.check_fdxdu(xk, uk, k[, delta=1e-5])¶

DSystem.check_fdudu(xk, uk, k[, delta=1e-5])¶

Parameters:	xk (numpy array, shape (nX)) – A valid state of the system uk (numpy array, shape (nU)) – A valid input to the system k (int) – A valid discrete time index delta – The perturbation for approximating the derivative.

These functions check derivatives of the discrete state dynamics against numeric approximations generated from lower derivatives (e.g, fdx() from f(), and fdudu() from fdu()). A three point approximation is used:

approx_deriv = (f(x + delta) - f(x - delta)) / (2 * delta)

Each function returns a named tuple (error, exact_norm, approx_norm) where error is the norm of the difference between the exact and approximate derivative, exact_norm is the norm of the exact derivative, approx_norm is the norm of the approximate derivative.

DSystem - Discrete System wrapper for Midpoint Variational Integrators¶

DSystem Objects¶

State and Trajectory Manipulation¶

Dynamics¶

First Derivatives¶

Second Derivatives¶

Linearization and Feedback Controllers¶

Checking the Derivatives¶