Skip to content

Commit dd9019a

Browse files
committed
Update JacobianData struct
1 parent 112d884 commit dd9019a

File tree

5 files changed

+62
-38
lines changed

5 files changed

+62
-38
lines changed

Project.toml

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -23,11 +23,11 @@ SparseDiffTools = "47a9eef4-7e08-11e9-0b38-333d64bd3804"
2323
StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
2424

2525
[compat]
26-
Colors = "0.12"
26+
Colors = "0.12, 0.13"
2727
ForwardDiff = "0.10"
28-
GeometryBasics = "0.4"
28+
GeometryBasics = "0.4, 0.5"
2929
Ipopt = "1"
30-
MeshCat = "0.14, 0.15, 0.16"
30+
MeshCat = "0.14, 0.15, 0.16, 1"
3131
MeshCatMechanisms = "0.8, 0.9"
3232
NPZ = "0.4"
3333
Plots = "1"

src/constraints/dynamics.jl

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -125,8 +125,8 @@ function make_cb_eval_ga_con_dyn(problem, robot, jacdata)
125125
jacdata(x[ind_vars])
126126

127127
# Pass the Jacobian to Knitro
128-
offset_jac = i * jacdata.length_jac
129-
ind_jac = (1:jacdata.length_jac) .+ offset_jac
128+
offset_jac = i * jacdata.jac_length
129+
ind_jac = (1:jacdata.jac_length) .+ offset_jac
130130
evalResult.jac[ind_jac] = nonzeros(jacdata.jac)
131131
end
132132

src/constraints/end_effector.jl

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ function cb_eval_ga_con_ee(kc, cb, evalRequest, evalResult, userParams)
3434
for (i, k) = enumerate(sort(collect(keys(problem.ee_pos))))
3535
ind_qᵢ = range(1 + (k - 1) * nₓ, length=robot.n_q) # indices of the decision variables
3636
problem.jacdata_ee_position(x[ind_qᵢ]) # jacobian evaluation at that point
37-
ind_jac = (1:problem.jacdata_ee_position.length_jac) .+ ((i - 1) * problem.jacdata_ee_position.length_jac)
37+
ind_jac = (1:problem.jacdata_ee_position.jac_length) .+ ((i - 1) * problem.jacdata_ee_position.jac_length)
3838
evalResult.jac[ind_jac] = nonzeros(problem.jacdata_ee_position.jac) # pass jacobian to Knitro
3939
end
4040

src/jacobian_data.jl

Lines changed: 49 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1,35 +1,59 @@
1-
struct JacobianData{T1 <: SparseMatrixCSC,T2 <: ForwardColorJacCache}
2-
f!::Function
3-
jac::T1
4-
jac_cache::T2
5-
length_jac::Int
6-
sparsity::SparseArrays.SparseMatrixCSC{Bool,Int64}
7-
8-
function JacobianData(f!, output, input)
9-
# TODO Proper way to do it (currently not working)
10-
# Issue: https://github.com/SciML/SparsityDetection.jl/issues/41
11-
# sparsity = jacobian_sparsity(f!, output, input)
12-
13-
# Workaround
1+
import SparseArrays
2+
import SparseDiffTools
3+
# import Symbolics
4+
5+
"""
6+
`JacobianData` is a utility strcture to hold Jacobian information associated with a given function.
7+
"""
8+
struct JacobianData{T,F<:Function,C<:SparseDiffTools.ForwardColorJacCache}
9+
f!::F
10+
jac::SparseArrays.SparseMatrixCSC{T,Int}
11+
jac_cache::C
12+
jac_length::Int
13+
jac_sparsity::SparseArrays.SparseMatrixCSC{Bool,Int}
14+
15+
@doc """
16+
JacobianData(f!, output, input)
17+
18+
Create a new `JacobianData`.
19+
20+
# Arguments
21+
- `f!`: the method associated with this `JacobianData`
22+
- `output`: input vector to be passed to `f!`
23+
- `input`: output vector expected from `f!`
24+
"""
25+
function JacobianData(f!::F, output::Vector{T}, input::Vector{T}) where {T,F<:Function}
26+
# # Issue: https://github.com/SciML/SparsityDetection.jl/issues/41
27+
# # Calculate the sparsity pattern of the Jacobian
28+
# jac_sparsity = Symbolics.jacobian_sparsity(f!, output, input)
29+
30+
# [HACKY!] Calculate the sparsity pattern of the Jacobian
1431
num_samples = 50
1532
jac_samples = [ForwardDiff.jacobian(f!, rand!(output), rand!(input)) for _ in 1:num_samples]
16-
sparsity = sparse(sum(jac_samples) .≠ 0)
33+
jac_sparsity = SparseArrays.sparse(sum(jac_samples) .≠ 0)
1734

18-
jac = convert.(Float64, sparse(sparsity))
35+
# Placeholder for the actual Jacobian
36+
jac = T.(jac_sparsity)
1937

20-
jac_cache = ForwardColorJacCache(f!, input, dx=output,
21-
colorvec=matrix_colors(jac),
22-
sparsity=sparsity)
38+
# Color the sparse matrix using graphical techniques (colorvec-assisted differentiation is significantly faster)
39+
colorvec = SparseDiffTools.matrix_colors(jac)
2340

24-
length_jac = nnz(jac)
41+
# Construct the color cache in advance, in order to not compute it each time the Jacobian needs to be evaluated
42+
jac_cache = SparseDiffTools.ForwardColorJacCache(f!, input, dx=output, colorvec=colorvec, sparsity=jac_sparsity)
2543

26-
T1 = typeof(jac)
27-
T2 = typeof(jac_cache)
44+
# The length of the Jacobian, i.e., the number of non-zero elements
45+
jac_length = SparseArrays.nnz(jac)
2846

29-
new{T1,T2}(f!, jac, jac_cache, length_jac, sparsity)
47+
C = typeof(jac_cache)
48+
49+
new{T,F,C}(f!, jac, jac_cache, jac_length, jac_sparsity)
3050
end
3151
end
3252

33-
function (jd::JacobianData)(x)
34-
forwarddiff_color_jacobian!(jd.jac, jd.f!, x, jd.jac_cache)
35-
end
53+
"""
54+
Evaluate this `JacobianData`'s function `f!` for point `x` using forward-mode automatic differentiation.
55+
56+
This method's syntax is "special". For more info on Function-like-objects, read the
57+
[docs](https://docs.julialang.org/en/v1/manual/methods/#Function-like-objects).
58+
"""
59+
(jd::JacobianData)(x) = SparseDiffTools.forwarddiff_color_jacobian!(jd.jac, jd.f!, x, jd.jac_cache)

src/transcription/ipopt.jl

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -187,20 +187,20 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
187187
jacdata_dyn(x[ind_vars])
188188

189189
# Pass the Jacobian to Knitro
190-
offset_jac = i * jacdata_dyn.length_jac
191-
ind_jac = (1:jacdata_dyn.length_jac) .+ offset_jac
190+
offset_jac = i * jacdata_dyn.jac_length
191+
ind_jac = (1:jacdata_dyn.jac_length) .+ offset_jac
192192
values[ind_jac] = nonzeros(jacdata_dyn.jac)
193193
end
194-
offset_prev += (problem.num_knots - 1) * jacdata_dyn.length_jac
194+
offset_prev += (problem.num_knots - 1) * jacdata_dyn.jac_length
195195
end
196196

197197
if use_m₂
198198
# End-effector constraints
199199
for (i, k) = enumerate(sort(collect(keys(problem.ee_pos))))
200200
ind_qᵢ = range(1 + (k - 1) * nₓ, length=robot.n_q) # indices of the decision variables
201201
problem.jacdata_ee_position(x[ind_qᵢ]) # jacobian evaluation at that point
202-
offset_jac = (i - 1) * problem.jacdata_ee_position.length_jac + offset_prev
203-
ind_jac = (1:problem.jacdata_ee_position.length_jac) .+ offset_jac
202+
offset_jac = (i - 1) * problem.jacdata_ee_position.jac_length + offset_prev
203+
ind_jac = (1:problem.jacdata_ee_position.jac_length) .+ offset_jac
204204
values[ind_jac] = nonzeros(problem.jacdata_ee_position.jac) # pass jacobian to Knitro
205205
end
206206
end
@@ -242,8 +242,8 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
242242

243243
# number of nonzeros in the Jacobian of the constraints
244244
nele_jac = 0
245-
nele_jac += !use_m₁ ? 0 : jacdata_dyn.length_jac * (problem.num_knots - 1)
246-
nele_jac += !use_m₂ ? 0 : problem.jacdata_ee_position.length_jac * length(problem.ee_pos)
245+
nele_jac += !use_m₁ ? 0 : jacdata_dyn.jac_length * (problem.num_knots - 1)
246+
nele_jac += !use_m₂ ? 0 : problem.jacdata_ee_position.jac_length * length(problem.ee_pos)
247247

248248
prob = Ipopt.CreateIpoptProblem(n, vec(x_L), vec(x_U), m, g_L, g_U, nele_jac, 0,
249249
eval_f, eval_g, eval_grad_f, eval_jac_g, nothing)

0 commit comments

Comments
 (0)