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:
DCost
defines the costs \(\ell(x, u, k)\) and
\(m(x)\) for a system and calculates their 1st and 2nd derivatives.
The current implementation defines a suitable cost for tracking a desired trajectory:
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.
1st 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.
2nd 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.