$\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}$

DCost - Discrete Trajectory Cost

The DCost class defines the incremental and terminal costs of a trajectory during a discrete trajectory optimization. It is used in conjunction with DSystem and DOptimizer.

The discrete trajectory optimization finds a trajectory that minimizes a cost of the form:

\[h(\xi) = \sum_{k=0}^{k_f-1} \ell(x(k), u(k), k) + m(x(k_f))\]

DCost defines the costs \(\ell(x, u, k)\) and \(m(x)\) for a system and calculates their 1^st and 2^nd derivatives.

The current implementation defines a suitable cost for tracking a desired trajectory:

\[ \begin{align}\begin{aligned}\ell(x, u, k) = \frac{1}{2}\left((x - x_d(k))^T Q (x - x_d(k)) + (x - u_d(k))^T R (u - u_d(k))\right)\\m(x) = \frac{1}{2}(x - x_d(k_f))^T Q (x - x_d(k_f))\end{aligned}\end{align} \]

where \(x_d(k)\) and \(u_d(k)\) are the desired state and input trajectories and \(Q\) and \(R\) are positive definite matrices that define their weighting.

DCost Objects

class trep.discopt.DCost(xd, ud, Q, R)

Parameters:

• xd (numpy array, shape (N, nX)) – The desired state trajectory
• ud (numpy array, shape (N-1, nU)) – The desired input trajectory
• Q (numpy array, shape (nX, nX)) – Cost weights for the states
• R (numpy array, shape (nU, nU)) – Cost weights for the inputs

Create a new cost object for the desired states xd weighted by Q and the desired inputs ud weighted by R.

DCost.Q

(numpy array, shape (nX, nX))

The weights of the states.

DCost.R

(numpy array, shape (nU, nU))

The weights of the inputs.

Costs

DCost.l(xk, uk, k)

Parameters:

• xk (numpy array, shape (nX)) – Current state
• uk (numpy array, shape (nU)) – Current input
• k (int) – Current discrete time

Return type:

float

Calculate the incremental cost of xk and uk at discrete time k.

DCost.m(xkf)

Parameters:xkf (numpy array, shape (nX)) – Final state Return type:float

Calculate the terminal cost of xk.

1^st Derivatives

DCost.l_dx(xk, uk, k)

Parameters:

• xk (numpy array, shape (nX)) – Current state
• uk (numpy array, shape (nU)) – Current input
• k (int) – Current discrete time

Return type:

numpy array, shape (nX)

Calculate the derivative of the incremental cost with respect to the state.

DCost.l_du(xk, uk, k)

Parameters:

• xk (numpy array, shape (nX)) – Current state
• uk (numpy array, shape (nU)) – Current input
• k (int) – Current discrete time

Return type:

numpy array, shape (nU)

Calculate the derivative of the incremental cost with respect to the input.

DCost.m_dx(xkf)

Parameters:xkf (numpy array, shape (nX)) – Current state Return type:numpy array, shape (nX)

Calculate the derivative of the terminal cost with respect to the final state.

2^nd Derivatives

DCost.l_dxdx(xk, uk, k)

Parameters:

• xk (numpy array, shape (nX)) – Current state
• uk (numpy array, shape (nU)) – Current input
• k (int) – Current discrete time

Return type:

numpy array, shape (nX, nX)

Calculate the second derivative of the incremental cost with respect to the state. For this implementation, this is always equal to Q.

DCost.l_dudu(xk, uk, k)

Parameters:

• xk (numpy array, shape (nX)) – Current state
• uk (numpy array, shape (nU)) – Current input
• k (int) – Current discrete time

Return type:

numpy array, shape (nU, nU)

Calculate the second derivative of the incremental cost with respect to the inputs. For this implementation, this is always equal to R.

DCost.l_dxdu(xk, uk, k)

Parameters:

• xk (numpy array, shape (nX)) – Current state
• uk (numpy array, shape (nU)) – Current input
• k (int) – Current discrete time

Return type:

numpy array, shape (nX, nU)

Calculate the second derivative of the incremental cost with respect to the state and inputs. For this implementation, this is always equal to zero.

DCost.m_dxdx(xkf)

Parameters:xkf (numpy array, shape (nX)) – Current state Return type:numpy array, shape (nX, nX)

Calculate the second derivative of the terminal cost. For this implementation, this is always equal to Q.