Incorrect Dense Costs on v0.7

[jbwillis]

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

Output:

julia> include("trajopt_costs_issue.jl")
xf_dense = [-1.0, 0.0]
RobotDynamics.evaluate(obj_dense[end], xf_dense, zeros(Nu)) = 9.0
RobotDynamics.evaluate(obj_dense[end], zeros(Nx), zeros(Nu)) = 5.0
xf_static = [-1.0, 0.0]
RobotDynamics.evaluate(obj_static[end], xf_static, zeros(Nu)) = 0.0
RobotDynamics.evaluate(obj_static[end], zeros(Nx), zeros(Nu)) = 5.0
ERROR: LoadError: AssertionError: RobotDynamics.evaluate(obj_static[end], xf_static, zeros(Nu)) == RobotDynamics.evaluate(obj_dense[end], xf_dense, zeros(Nu))

[jbwillis]

Resolved in #67