-
Notifications
You must be signed in to change notification settings - Fork 10
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #6 from JuliaRobotics/dev-0.3.x
TORA.jl v0.3
- Loading branch information
Showing
13 changed files
with
187 additions
and
79 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,18 +1,20 @@ | ||
| name = "TORA" | ||
| uuid = "a6da6c0f-f153-4ec6-bf42-6dc0ab733f84" | ||
| authors = ["Henrique Ferrolho <[email protected]>"] | ||
| version = "0.2.0" | ||
| version = "0.3.0" | ||
|
|
||
| [deps] | ||
| Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" | ||
| ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" | ||
| GeometryBasics = "5c1252a2-5f33-56bf-86c9-59e7332b4326" | ||
| HSL_jll = "017b0a0e-03f4-516a-9b91-836bbd1904dd" | ||
| Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" | ||
| MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" | ||
| MeshCatMechanisms = "6ad125db-dd91-5488-b820-c1df6aab299d" | ||
| NPZ = "15e1cf62-19b3-5cfa-8e77-841668bca605" | ||
| Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" | ||
| Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" | ||
| PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a" | ||
| Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" | ||
| Requires = "ae029012-a4dd-5104-9daa-d747884805df" | ||
| RigidBodyDynamics = "366cf18f-59d5-5db9-a4de-86a9f6786172" | ||
|
|
@@ -21,19 +23,20 @@ SparseDiffTools = "47a9eef4-7e08-11e9-0b38-333d64bd3804" | |
| StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" | ||
|
|
||
| [compat] | ||
| Colors = "0.12" | ||
| Colors = "0.12, 0.13" | ||
| ForwardDiff = "0.10" | ||
| GeometryBasics = "0.4" | ||
| GeometryBasics = "0.4, 0.5" | ||
| Ipopt = "1" | ||
| MeshCat = "0.14" | ||
| MeshCatMechanisms = "0.8" | ||
| MeshCat = "0.14, 0.15, 0.16, 1" | ||
| MeshCatMechanisms = "0.8, 0.9" | ||
| NPZ = "0.4" | ||
| Plots = "1" | ||
| PrecompileTools = "1" | ||
| Requires = "1" | ||
| RigidBodyDynamics = "2" | ||
| SparseDiffTools = "1" | ||
| SparseDiffTools = "2" | ||
| StaticArrays = "1" | ||
| julia = "1.6" | ||
| julia = "1.8" | ||
|
|
||
| [extras] | ||
| Aqua = "4c88cf16-eb10-579e-8560-4a9242c79595" | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,35 +1,59 @@ | ||
| struct JacobianData{T1 <: SparseMatrixCSC,T2 <: ForwardColorJacCache} | ||
| f!::Function | ||
| jac::T1 | ||
| jac_cache::T2 | ||
| length_jac::Int | ||
| sparsity::SparseArrays.SparseMatrixCSC{Bool,Int64} | ||
|
|
||
| function JacobianData(f!, output, input) | ||
| # TODO Proper way to do it (currently not working) | ||
| # Issue: https://github.com/SciML/SparsityDetection.jl/issues/41 | ||
| # sparsity = jacobian_sparsity(f!, output, input) | ||
|
|
||
| # Workaround | ||
| import SparseArrays | ||
| import SparseDiffTools | ||
| # import Symbolics | ||
|
|
||
| """ | ||
| `JacobianData` is a utility strcture to hold Jacobian information associated with a given function. | ||
| """ | ||
| struct JacobianData{T,F<:Function,C<:SparseDiffTools.ForwardColorJacCache} | ||
| f!::F | ||
| jac::SparseArrays.SparseMatrixCSC{T,Int} | ||
| jac_cache::C | ||
| jac_length::Int | ||
| jac_sparsity::SparseArrays.SparseMatrixCSC{Bool,Int} | ||
|
|
||
| @doc """ | ||
| JacobianData(f!, output, input) | ||
| Create a new `JacobianData`. | ||
| # Arguments | ||
| - `f!`: the method associated with this `JacobianData` | ||
| - `output`: input vector to be passed to `f!` | ||
| - `input`: output vector expected from `f!` | ||
| """ | ||
| function JacobianData(f!::F, output::Vector{T}, input::Vector{T}) where {T,F<:Function} | ||
| # # Issue: https://github.com/SciML/SparsityDetection.jl/issues/41 | ||
| # # Calculate the sparsity pattern of the Jacobian | ||
| # jac_sparsity = Symbolics.jacobian_sparsity(f!, output, input) | ||
|
|
||
| # [HACKY!] Calculate the sparsity pattern of the Jacobian | ||
| num_samples = 50 | ||
| jac_samples = [ForwardDiff.jacobian(f!, rand!(output), rand!(input)) for _ in 1:num_samples] | ||
| sparsity = sparse(sum(jac_samples) .≠ 0) | ||
| jac_sparsity = SparseArrays.sparse(sum(jac_samples) .≠ 0) | ||
|
|
||
| jac = convert.(Float64, sparse(sparsity)) | ||
| # Placeholder for the actual Jacobian | ||
| jac = T.(jac_sparsity) | ||
|
|
||
| jac_cache = ForwardColorJacCache(f!, input, dx=output, | ||
| colorvec=matrix_colors(jac), | ||
| sparsity=sparsity) | ||
| # Color the sparse matrix using graphical techniques (colorvec-assisted differentiation is significantly faster) | ||
| colorvec = SparseDiffTools.matrix_colors(jac) | ||
|
|
||
| length_jac = nnz(jac) | ||
| # Construct the color cache in advance, in order to not compute it each time the Jacobian needs to be evaluated | ||
| jac_cache = SparseDiffTools.ForwardColorJacCache(f!, input, dx=output, colorvec=colorvec, sparsity=jac_sparsity) | ||
|
|
||
| T1 = typeof(jac) | ||
| T2 = typeof(jac_cache) | ||
| # The length of the Jacobian, i.e., the number of non-zero elements | ||
| jac_length = SparseArrays.nnz(jac) | ||
|
|
||
| new{T1,T2}(f!, jac, jac_cache, length_jac, sparsity) | ||
| C = typeof(jac_cache) | ||
|
|
||
| new{T,F,C}(f!, jac, jac_cache, jac_length, jac_sparsity) | ||
| end | ||
| end | ||
|
|
||
| function (jd::JacobianData)(x) | ||
| forwarddiff_color_jacobian!(jd.jac, jd.f!, x, jd.jac_cache) | ||
| end | ||
| """ | ||
| Evaluate this `JacobianData`'s function `f!` for point `x` using forward-mode automatic differentiation. | ||
| This method's syntax is "special". For more info on Function-like-objects, read the | ||
| [docs](https://docs.julialang.org/en/v1/manual/methods/#Function-like-objects). | ||
| """ | ||
| (jd::JacobianData)(x) = SparseDiffTools.forwarddiff_color_jacobian!(jd.jac, jd.f!, x, jd.jac_cache) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,13 @@ | ||
| import PrecompileTools | ||
|
|
||
| PrecompileTools.@compile_workload begin | ||
| vis = Visualizer() | ||
| robot = create_robot_franka("panda_arm", vis) | ||
| problem = Problem(robot, 201, 1/100) | ||
| fix_joint_velocities!(problem, robot, 1, zeros(robot.n_v)) | ||
| cpu_time, x, solver_log = solve_with_ipopt(problem, robot) | ||
| play_trajectory(vis, problem, robot, x) | ||
| plot_results(problem, robot, x) | ||
| Plots.closeall() | ||
| MeshCat.close_server!(vis.core) | ||
| end |
Oops, something went wrong.
7a4e658There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@JuliaRegistrator register
7a4e658There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Registration pull request created: JuliaRegistries/General/119797
Tip: Release Notes
Did you know you can add release notes too? Just add markdown formatted text underneath the comment after the text
"Release notes:" and it will be added to the registry PR, and if TagBot is installed it will also be added to the
release that TagBot creates. i.e.
To add them here just re-invoke and the PR will be updated.
Tagging
After the above pull request is merged, it is recommended that a tag is created on this repository for the registered package version.
This will be done automatically if the Julia TagBot GitHub Action is installed, or can be done manually through the github interface, or via: