I have found that using dense (diagonal) matrices results in an incorrectly computed cost. It seems that the cost is computed without subtracting xf. See the code and output. The assert that fails is on the last line of the code. It shows that costs computed using static arrays and dense matrices should be the same, but are not.
using TrajectoryOptimization
using RobotDynamics
using StaticArrays
using LinearAlgebra
let
Nx = 2 # state dimension
Nu = 2 # control dimension
Nk = 100 # timesteps
Qx_dense = diagm(ones(Nx))
QxT_dense = 10*diagm(ones(Nx))
R_dense = 0.5*diagm(ones(Nu))
xf_dense = zeros(Nx)
xf_dense[1] = -1
@show xf_dense
obj_dense = LQRObjective(Qx_dense, R_dense, QxT_dense, xf_dense, Nk)
@show RobotDynamics.evaluate(obj_dense[end], xf_dense, zeros(Nu))
@show RobotDynamics.evaluate(obj_dense[end], zeros(Nx), zeros(Nu));
Qx_static = (@SVector ones(Nx))
QxT_static = 10*(@SVector ones(Nx))
R_static = 0.5*(@SVector ones(Nu))
xf_static = [ -1; @SVector zeros(Nx-1)]
@show xf_static
obj_static = LQRObjective(Qx_static, R_static, QxT_static, xf_static, Nk)
@show RobotDynamics.evaluate(obj_static[end], xf_static, zeros(Nu))
@show RobotDynamics.evaluate(obj_static[end], zeros(Nx), zeros(Nu));
# cost function elements are the same
@assert xf_dense == xf_static
@assert diag(Qx_dense) == Qx_static
@assert diag(QxT_dense) == QxT_static
@assert diag(R_dense) == R_static
# but the costs are not
@assert RobotDynamics.evaluate(obj_static[end], xf_static, zeros(Nu)) == RobotDynamics.evaluate(obj_dense[end], xf_dense, zeros(Nu))
end
I have found that using dense (diagonal) matrices results in an incorrectly computed cost. It seems that the cost is computed without subtracting
xf
. See the code and output. The assert that fails is on the last line of the code. It shows that costs computed using static arrays and dense matrices should be the same, but are not.Output: