BoundsError when solving trajectory

[sscheele]

When I define a trajectory optimization problem and pass it into Altro, the solver ends up throwing a BoundsError. I think it's happening when the solver tries to verify the constraints haven't been violated at the end of the solution. I say this because I played around with it a bit by changing the iterations_outer parameter to make the solver stop before it encountered the bug (it had been consistently crashing at iteration 17, so I set iterations_outer=15), and the bug continued to appear when iterations_outer had been reached. I've included the entire stack trace below, let me know if I can provide any more information:

ERROR: LoadError: BoundsError: attempt to access 693×273 SparseArrays.SparseMatrixCSC{Float64,Int64} at index [StaticArrays.StaticIndexing{SArray{Tuple{14},Int64,1,14}}([680, 681, 682, 683, 684, 685, 686, 687, 688, 689, 690, 691, 692, 693]), StaticArrays.StaticIndexing{SArray{Tuple{14},Int64,1,14}}([267, 268, 269, 270, 271, 272, 273, 274, 275, 276, 277, 278, 279, 280])]
Stacktrace:
 [1] throw_boundserror(::SparseArrays.SparseMatrixCSC{Float64,Int64}, ::Tuple{StaticArrays.StaticIndexing{SArray{Tuple{14},Int64,1,14}},StaticArrays.StaticIndexing{SArray{Tuple{14},Int64,1,14}}}) at ./abstractarray.jl:541
 [2] checkbounds at ./abstractarray.jl:506 [inlined]
 [3] view at ./subarray.jl:158 [inlined]
 [4] maybeview at ./views.jl:133 [inlined]
 [5] dotview at ./broadcast.jl:1160 [inlined]
 [6] copy_jacobian!(::SparseArrays.SparseMatrixCSC{Float64,Int64}, ::TrajectoryOptimization.ConVal{BoundConstraint{14,14,Float64},MArray{Tuple{14},Float64,1,14},SizedArray{Tuple{14,14},Float64,2,2},SubArray{Float64,2,SizedArray{Tuple{14,14},Float64,2,2},Tuple{Base.Slice{SOneTo{14}},UnitRange{Int64}},true}}, ::Array{SArray{Tuple{14},Int64,1,14},1}, ::Array{SArray{Tuple{7},Int64,1,7},1}, ::Array{SArray{Tuple{7},Int64,1,7},1}) at /home/sam/.julia/packages/Altro/scOHW/src/direct/copy_blocks.jl:59
 [7] copy_jacobians!(::SparseArrays.SparseMatrixCSC{Float64,Int64}, ::Altro.ProjectedNewtonSolver{Float64,7,7,14}) at /home/sam/.julia/packages/Altro/scOHW/src/direct/copy_blocks.jl:102
 [8] copy_jacobians! at /home/sam/.julia/packages/Altro/scOHW/src/direct/pn_methods.jl:249 [inlined]
 [9] solve!(::Altro.ProjectedNewtonSolver{Float64,7,7,14}) at /home/sam/.julia/packages/Altro/scOHW/src/direct/pn_methods.jl:8
 [10] solve!(::ALTROSolver{Float64,iLQRSolver{Float64,RK3,Kuka,Altro.ALObjective{Float64,Objective{GeneralCost{7,7}}},7,7,7,14}}) at /home/sam/.julia/packages/Altro/scOHW/src/altro/altro_solver.jl:99
 [11] main() at /home/sam/comoto/src/comoto.jl:438
 [12] top-level scope at /home/sam/comoto/src/comoto.jl:444
 [13] include(::Function, ::Module, ::String) at ./Base.jl:380
 [14] include(::Module, ::String) at ./Base.jl:368
 [15] exec_options(::Base.JLOptions) at ./client.jl:296
 [16] _start() at ./client.jl:506
in expression starting at /home/sam/comoto/src/comoto.jl:444

[bjack205]

It looks like this is happening within the projected newton solve. I think I've run into this problem before. Can you provide a little more detail about the problem you're solving? In particular, the constraints you have on the problem? I think this type of problem usually arises for me when I have redundant constraints.

[sscheele]

Sure - here's a link to the code with the constraints highlighted. Lines 422 and 423 result in the error appearing when they're uncommented.

https://github.com/sscheele/comoto.jl/blob/master/src/comoto.jl#L421-L423

The constraints are: joint position goal constraint, control magnitude constraints (for my case, it's joint velocity control, so this is equivalent to joint velocity constraints), and joint limit constraints, in the order they appear in the code.

Thanks for your help!

[bjack205]

Ah, yes. You have redundant constraints at the last time step, since you have a goal constraint and bound constraints. Try changing the indices of your state bound constraint to only apply up to the penultimate knot point. That should fix your issue, hopefully.

Admittedly, I need to figure out a better way of detecting this type of constraint redundancy up front.

[sscheele]

This worked, thank you!