Merge pull request #6 from JuliaRobotics/dev-0.3.x

TORA.jl v0.3

Author: ferrolho

.github/workflows/Documentation.yml

@@ -16,7 +16,7 @@ jobs:
       - uses: actions/checkout@v2
       - uses: julia-actions/setup-julia@v1
         with:
-          version: '1.6'
+          version: '1.8'
       - name: Install dependencies
         run: julia --project=docs/ -e 'using Pkg; Pkg.develop(PackageSpec(path=pwd())); Pkg.instantiate()'
       - name: Build and deploy

Project.toml

@@ -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"

docs/make.jl

@@ -5,6 +5,7 @@ using Documenter, TORA
 makedocs(
     modules = [TORA],
     format = Documenter.HTML(),
+    warnonly = Documenter.except(),
     sitename = "TORA.jl",
     authors = "Henrique Ferrolho",
     pages = [

notebooks/Example.ipynb

@@ -27,8 +27,11 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "# Workaround for: https://github.com/JuliaRobotics/RigidBodyDynamics.jl/issues/500\n",
-    "using LinearAlgebra; BLAS.set_num_threads(1)"
+    "using LinearAlgebra\n",
+    "# Try to load something faster than OpenBLAS\n",
+    "try using AppleAccelerate catch e; end\n",
+    "try using MKL catch e; end\n",
+    "BLAS.get_config()"
    ]
   },
   {
@@ -37,11 +40,23 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "using KNITRO\n",
+    "# using KNITRO\n",
     "using MeshCat\n",
     "using RigidBodyDynamics"
    ]
   },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Load the pre-compiled binaries of Coin HSL routines\n",
+    "HSL_jll_path = expanduser(\"~/HSL_jll.jl-2023.5.26\")\n",
+    "Pkg.develop(path=HSL_jll_path)\n",
+    "import HSL_jll"
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": null,
@@ -71,8 +86,13 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "robot = TORA.create_robot_kuka_iiwa_14(vis)\n",
-    "problem = TORA.Problem(robot, 301, 1/150)\n",
+    "robot = TORA.create_robot_franka(\"panda_arm\", vis)\n",
+    "# robot = TORA.create_robot_kinova_gen2(\"j2s6s200\", vis)\n",
+    "# robot = TORA.create_robot_kinova_gen3(\"gen3_robotiq_2f_140\", vis)\n",
+    "# robot = TORA.create_robot_kuka(\"iiwa14\", vis)\n",
+    "# robot = TORA.create_robot_ur(\"ur10e\", vis)\n",
+    "\n",
+    "problem = TORA.Problem(robot, 2001, 1/1000)\n",
     "\n",
     "# Constrain initial and final joint velocities to zero\n",
     "TORA.fix_joint_velocities!(problem, robot, 1, zeros(robot.n_v))\n",
@@ -90,7 +110,8 @@
     "\n",
     "    for k = 1:2:problem.num_knots\n",
     "        θ = CubicTimeScaling(problem.num_knots - 1, k - 1) * 2π\n",
-    "        pos = [0.5, 0.2 * cos(θ), 0.8 + 0.2 * sin(θ)]\n",
+    "        # pos = [0.5, 0.2 * cos(θ), 0.8 + 0.2 * sin(θ)]  # UR10e\n",
+    "        pos = [0.4, 0.2 * cos(θ), 0.7 + 0.2 * sin(θ)]  # Franka Emika\n",
     "        # pos = [0.5, 0.3 * sin(θ) + 0.1 * sin(8 * θ), 0.8 + 0.3 * cos(θ) + 0.1 * cos(8 * θ)]\n",
     "        TORA.constrain_ee_position!(problem, k, pos)\n",
     "    end\n",
@@ -105,7 +126,11 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "initial_q = [0, 0, 0, -π/2, 0, 0, 0]\n",
+    "initial_q = zeros(robot.n_q)\n",
+    "# initial_q = [0, 0, 0, -π/2, 0, 0, 0]\n",
+    "initial_q = [0, -π/4, 0, -3π/4, 0, π/2, 0]  # Franka Emika\n",
+    "# initial_q = [0, -120, 120, -180, -90, 0] .|> deg2rad  # UR10e\n",
+    "# initial_q = x[1:7]\n",
     "\n",
     "zero!(robot.state)\n",
     "set_configuration!(robot.state, initial_q)\n",
@@ -130,6 +155,45 @@
     "use_inv_dyn = true\n",
     "minimise_τ = false\n",
     "\n",
+    "user_options = Dict(\n",
+    "    # === Termination === #  https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Termination\n",
+    "    # \"tol\" => 10e-2,  # default: 10e-8\n",
+    "    # \"max_cpu_time\" => 4.0,  # default: 10e20\n",
+    "    # \"constr_viol_tol\" => 0.1,  # default: 0.0001\n",
+    "    # \"acceptable_tol\" => 0.1,  # default: 10e-6\n",
+    "    # \"acceptable_constr_viol_tol\" => 0.1,  # default: 0.01\n",
+    "\n",
+    "    # === Output === #  https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Output\n",
+    "    # \"print_level\" => 0,  # [0, 12], default: 5\n",
+    "    \n",
+    "    # === NLP Scaling === #  https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_NLP_Scaling\n",
+    "    # \"nlp_scaling_method\" => \"none\",  # none, user-scaling, gradient-based (default), equilibration-based\n",
+    "\n",
+    "    # === Warm Start === #  https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Warm_Start\n",
+    "    # \"warm_start_init_point\" => \"yes\",\n",
+    "    # \"warm_start_same_structure\" => \"yes\",\n",
+    "    # \"warm_start_entire_iterate\" => \"yes\",\n",
+    "\n",
+    "    # === Barrier Parameter Update === #  https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Barrier_Parameter_Update\n",
+    "    \"mu_strategy\" => \"adaptive\",  # monotone (default), adaptive\n",
+    "    # \"mu_oracle\" => \"loqo\",  # probing, loqo, quality-function (default)\n",
+    "\n",
+    "    # === Linear Solver === #  https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Linear_Solver\n",
+    "    \"linear_solver\" => \"ma57\",  # ma27 (default), ma57, ma77, ma86, ma97, (...)\n",
+    "    # \"ma57_pre_alloc\" => 1.10,  # [1, Inf), 1.05 (default)\n",
+    "\n",
+    "    # === Step Calculation === #  https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Step_Calculation\n",
+    "    # \"mehrotra_algorithm\" => \"yes\",  # yes, no (default)\n",
+    "    \"fast_step_computation\" => \"yes\",  # yes, no (default)\n",
+    "\n",
+    "    # === Hessian Approximation === #  https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Hessian_Approximation\n",
+    "    \"hessian_approximation\" => \"limited-memory\",  # exact (default), limited-memory\n",
+    "    # \"hessian_approximation_space\" => \"all-variables\", # nonlinear-variables (default), all-variables\n",
+    "\n",
+    "    # === Derivative Checker === #  https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Derivative_Checker\n",
+    "    # \"derivative_test\" => \"first-order\",  # none, first-order, second-order, only-second-order\n",
+    ")\n",
+    "\n",
     "# Choose which solver you want to use:\n",
     "solve = TORA.solve_with_ipopt  # Uses Ipopt (https://github.com/coin-or/Ipopt)\n",
     "# solve = TORA.solve_with_knitro  # Uses KNITRO (https://www.artelys.com/solvers/knitro/)\n",
@@ -138,7 +202,8 @@
     "cpu_time, x, solver_log = solve(problem, robot,\n",
     "                                initial_guess=initial_guess,\n",
     "                                use_inv_dyn=use_inv_dyn,\n",
-    "                                minimise_τ=minimise_τ)"
+    "                                minimise_τ=minimise_τ,\n",
+    "                                user_options=user_options)"
    ]
   },
   {
@@ -162,7 +227,9 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "metadata": {},
+   "metadata": {
+    "tags": []
+   },
    "outputs": [],
    "source": [
     "TORA.plot_log(solver_log)"
@@ -171,15 +238,15 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Julia 1.7.2",
+   "display_name": "Julia 1.10.2",
    "language": "julia",
-   "name": "julia-1.7"
+   "name": "julia-1.10"
   },
   "language_info": {
    "file_extension": ".jl",
    "mimetype": "application/julia",
    "name": "julia",
-   "version": "1.7.2"
+   "version": "1.10.2"
   }
  },
  "nbformat": 4,

notebooks/Tutorial.ipynb

@@ -128,15 +128,15 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Julia 1.7.2",
+   "display_name": "Julia 1.10.2",
    "language": "julia",
-   "name": "julia-1.7"
+   "name": "julia-1.10"
   },
   "language_info": {
    "file_extension": ".jl",
    "mimetype": "application/julia",
    "name": "julia",
-   "version": "1.7.2"
+   "version": "1.10.2"
   }
  },
  "nbformat": 4,

src/TORA.jl

@@ -61,6 +61,9 @@ include("./constraints/end_effector.jl")
 include("./transcription/ipopt.jl")
 include("./plots.jl")
 
+# Code to "exercise" the package - see https://julialang.github.io/PrecompileTools.jl/stable/
+include("./precompile.jl")
+
 export solve_with_knitro
 
 end # module

src/constraints/dynamics.jl

@@ -125,8 +125,8 @@ function make_cb_eval_ga_con_dyn(problem, robot, jacdata)
             jacdata(x[ind_vars])
 
             # Pass the Jacobian to Knitro
-            offset_jac = i * jacdata.length_jac
-            ind_jac = (1:jacdata.length_jac) .+ offset_jac
+            offset_jac = i * jacdata.jac_length
+            ind_jac = (1:jacdata.jac_length) .+ offset_jac
             evalResult.jac[ind_jac] = nonzeros(jacdata.jac)
         end
 

src/constraints/end_effector.jl

@@ -34,7 +34,7 @@ function cb_eval_ga_con_ee(kc, cb, evalRequest, evalResult, userParams)
     for (i, k) = enumerate(sort(collect(keys(problem.ee_pos))))
         ind_qᵢ = range(1 + (k - 1) * nₓ, length=robot.n_q)  # indices of the decision variables
         problem.jacdata_ee_position(x[ind_qᵢ])              # jacobian evaluation at that point
-        ind_jac = (1:problem.jacdata_ee_position.length_jac) .+ ((i - 1) * problem.jacdata_ee_position.length_jac)
+        ind_jac = (1:problem.jacdata_ee_position.jac_length) .+ ((i - 1) * problem.jacdata_ee_position.jac_length)
         evalResult.jac[ind_jac] = nonzeros(problem.jacdata_ee_position.jac)  # pass jacobian to Knitro
     end
 

src/jacobian_data.jl

@@ -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)

src/precompile.jl

@@ -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