diff --git a/.github/workflows/Documentation.yml b/.github/workflows/Documentation.yml
index 7dac19d..c61eeeb 100644
--- a/.github/workflows/Documentation.yml
+++ b/.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
diff --git a/Project.toml b/Project.toml
index 95d1420..f45b3c7 100644
--- a/Project.toml
+++ b/Project.toml
@@ -1,18 +1,20 @@
 name = "TORA"
 uuid = "a6da6c0f-f153-4ec6-bf42-6dc0ab733f84"
 authors = ["Henrique Ferrolho <henriqueferrolho@gmail.com>"]
-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"
diff --git a/docs/make.jl b/docs/make.jl
index cddad50..c406919 100644
--- a/docs/make.jl
+++ b/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 = [
diff --git a/notebooks/Example.ipynb b/notebooks/Example.ipynb
index 487a2cc..f9405ff 100644
--- a/notebooks/Example.ipynb
+++ b/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,
diff --git a/notebooks/Tutorial.ipynb b/notebooks/Tutorial.ipynb
index 30349d3..b4f3915 100644
--- a/notebooks/Tutorial.ipynb
+++ b/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,
diff --git a/src/TORA.jl b/src/TORA.jl
index 03b0c75..a5da926 100644
--- a/src/TORA.jl
+++ b/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
diff --git a/src/constraints/dynamics.jl b/src/constraints/dynamics.jl
index 7b75e37..304d8f4 100644
--- a/src/constraints/dynamics.jl
+++ b/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
 
diff --git a/src/constraints/end_effector.jl b/src/constraints/end_effector.jl
index a607337..123b1cd 100644
--- a/src/constraints/end_effector.jl
+++ b/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
 
diff --git a/src/jacobian_data.jl b/src/jacobian_data.jl
index d22f9d7..5a63c3d 100644
--- a/src/jacobian_data.jl
+++ b/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)
diff --git a/src/precompile.jl b/src/precompile.jl
new file mode 100644
index 0000000..94509e9
--- /dev/null
+++ b/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
diff --git a/src/problem.jl b/src/problem.jl
index ee75fbe..8e32671 100644
--- a/src/problem.jl
+++ b/src/problem.jl
@@ -13,8 +13,8 @@ This happens in one of the `TORA.solve_with_*` functions, which does the heavy-l
 See also: [`solve_with_ipopt`](@ref), [`solve_with_knitro`](@ref)
 """
 mutable struct Problem
-    num_knots::Int  # Total number of knots
-    dt::Float64  # Time step between two knots
+    const num_knots::Int  # Total number of knots
+    const dt::Float64  # Time step between two knots
 
     fixed_q::Dict{Int64,Vector{Float64}}  # Fixed joint positions
     fixed_v::Dict{Int64,Vector{Float64}}  # Fixed joint velocities
@@ -23,9 +23,9 @@ mutable struct Problem
     ee_pos::Dict{Int64,Point{3,Float64}}  # End-effector target positions
 
     # Jacobian data (e.g., for sparse Jacobian of dynamics with AD)
-    jacdata_fwd_dyn::JacobianData
-    jacdata_inv_dyn::JacobianData
-    jacdata_ee_position::JacobianData
+    const jacdata_fwd_dyn::JacobianData
+    const jacdata_inv_dyn::JacobianData
+    const jacdata_ee_position::JacobianData
 
     @doc """
         Problem(robot, num_knots, dt)
diff --git a/src/transcription/ipopt.jl b/src/transcription/ipopt.jl
index 69587c5..a1e10ce 100644
--- a/src/transcription/ipopt.jl
+++ b/src/transcription/ipopt.jl
@@ -25,7 +25,7 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
                           initial_guess::Vector{Float64}=Float64[],
                           use_inv_dyn::Bool=false,
                           minimise_τ::Bool=false,
-                          user_options::Dict=Dict())
+                          user_options::Dict=Dict("hessian_approximation" => "limited-memory"))
     # # # # # # # # # # # # # # # #
     # Variables and their bounds  #
     # # # # # # # # # # # # # # # #
@@ -79,9 +79,6 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
 
     m = m₁ + m₂  # total number of constraints
 
-    ind_con_dyn    = (1:m₁)
-    ind_con_ee_pos = (1:m₂) .+ (m₁)
-
     g_L, g_U = Float64[], Float64[]
 
     if use_m₁
@@ -190,11 +187,11 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
                     jacdata_dyn(x[ind_vars])
 
                     # Pass the Jacobian to Knitro
-                    offset_jac = i * jacdata_dyn.length_jac
-                    ind_jac = (1:jacdata_dyn.length_jac) .+ offset_jac
+                    offset_jac = i * jacdata_dyn.jac_length
+                    ind_jac = (1:jacdata_dyn.jac_length) .+ offset_jac
                     values[ind_jac] = nonzeros(jacdata_dyn.jac)
                 end
-                offset_prev += (problem.num_knots - 1) * jacdata_dyn.length_jac
+                offset_prev += (problem.num_knots - 1) * jacdata_dyn.jac_length
             end
 
             if use_m₂
@@ -202,8 +199,8 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
                 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
-                    offset_jac = (i - 1) * problem.jacdata_ee_position.length_jac + offset_prev
-                    ind_jac = (1:problem.jacdata_ee_position.length_jac) .+ offset_jac
+                    offset_jac = (i - 1) * problem.jacdata_ee_position.jac_length + offset_prev
+                    ind_jac = (1:problem.jacdata_ee_position.jac_length) .+ offset_jac
                     values[ind_jac] = nonzeros(problem.jacdata_ee_position.jac)  # pass jacobian to Knitro
                 end
             end
@@ -245,8 +242,8 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
 
     # number of nonzeros in the Jacobian of the constraints
     nele_jac = 0
-    nele_jac += !use_m₁ ? 0 : jacdata_dyn.length_jac * (problem.num_knots - 1)
-    nele_jac += !use_m₂ ? 0 : problem.jacdata_ee_position.length_jac * length(problem.ee_pos)
+    nele_jac += !use_m₁ ? 0 : jacdata_dyn.jac_length * (problem.num_knots - 1)
+    nele_jac += !use_m₂ ? 0 : problem.jacdata_ee_position.jac_length * length(problem.ee_pos)
 
     prob = Ipopt.CreateIpoptProblem(n, vec(x_L), vec(x_U), m, g_L, g_U, nele_jac, 0,
                                     eval_f, eval_g, eval_grad_f, eval_jac_g, nothing)
diff --git a/src/transcription/knitro.jl b/src/transcription/knitro.jl
index 6b5f254..de6d7ad 100644
--- a/src/transcription/knitro.jl
+++ b/src/transcription/knitro.jl
@@ -26,21 +26,21 @@ function solve_with_knitro(problem::Problem, robot::Robot;
     ind_v = hcat([range(1 + (i * nₓ) + robot.n_q            , length=robot.n_v) for i = (1:problem.num_knots    ) .- 1]...)
     ind_τ = hcat([range(1 + (i * nₓ) + robot.n_q + robot.n_v, length=robot.n_τ) for i = (1:problem.num_knots - 1) .- 1]...)
 
-    KNITRO.KN_set_var_lobnds(kc, Cint.(vec(ind_q) .- 1), repeat(robot.q_lo, problem.num_knots))  # q lower bounds
-    KNITRO.KN_set_var_upbnds(kc, Cint.(vec(ind_q) .- 1), repeat(robot.q_hi, problem.num_knots))  # q upper bounds
+    KNITRO.KN_set_var_lobnds(kc, length(ind_q), Cint.(vec(ind_q) .- 1), repeat(robot.q_lo, problem.num_knots))  # q lower bounds
+    KNITRO.KN_set_var_upbnds(kc, length(ind_q), Cint.(vec(ind_q) .- 1), repeat(robot.q_hi, problem.num_knots))  # q upper bounds
 
-    KNITRO.KN_set_var_lobnds(kc, Cint.(vec(ind_v) .- 1), repeat(robot.v_lo, problem.num_knots))  # v lower bounds
-    KNITRO.KN_set_var_upbnds(kc, Cint.(vec(ind_v) .- 1), repeat(robot.v_hi, problem.num_knots))  # v upper bounds
+    KNITRO.KN_set_var_lobnds(kc, length(ind_v), Cint.(vec(ind_v) .- 1), repeat(robot.v_lo, problem.num_knots))  # v lower bounds
+    KNITRO.KN_set_var_upbnds(kc, length(ind_v), Cint.(vec(ind_v) .- 1), repeat(robot.v_hi, problem.num_knots))  # v upper bounds
 
     if !isempty(ind_τ)
-        KNITRO.KN_set_var_lobnds(kc, Cint.(vec(ind_τ) .- 1), repeat(robot.τ_lo, problem.num_knots - 1))  # τ lower bounds
-        KNITRO.KN_set_var_upbnds(kc, Cint.(vec(ind_τ) .- 1), repeat(robot.τ_hi, problem.num_knots - 1))  # τ upper bounds
+        KNITRO.KN_set_var_lobnds(kc, length(ind_τ), Cint.(vec(ind_τ) .- 1), repeat(robot.τ_lo, problem.num_knots - 1))  # τ lower bounds
+        KNITRO.KN_set_var_upbnds(kc, length(ind_τ), Cint.(vec(ind_τ) .- 1), repeat(robot.τ_hi, problem.num_knots - 1))  # τ upper bounds
     end
 
     # Fixed variables
-    for (k, q) ∈ problem.fixed_q KNITRO.KN_set_var_fxbnds(kc, Cint.(vec(ind_q[:,k]) .- 1), q) end
-    for (k, v) ∈ problem.fixed_v KNITRO.KN_set_var_fxbnds(kc, Cint.(vec(ind_v[:,k]) .- 1), v) end
-    for (k, τ) ∈ problem.fixed_τ KNITRO.KN_set_var_fxbnds(kc, Cint.(vec(ind_τ[:,k]) .- 1), τ) end
+    for (k, q) ∈ problem.fixed_q KNITRO.KN_set_var_fxbnds(kc, length(robot.n_q), Cint.(vec(ind_q[:,k]) .- 1), q) end
+    for (k, v) ∈ problem.fixed_v KNITRO.KN_set_var_fxbnds(kc, length(robot.n_v), Cint.(vec(ind_v[:,k]) .- 1), v) end
+    for (k, τ) ∈ problem.fixed_τ KNITRO.KN_set_var_fxbnds(kc, length(robot.n_τ), Cint.(vec(ind_τ[:,k]) .- 1), τ) end
 
     # # # # # # # #
     # Constraints #
@@ -59,7 +59,7 @@ function solve_with_knitro(problem::Problem, robot::Robot;
     ind_con_ee_pos = (1:m₂) .+ (m₁)
 
     if use_m₁
-        KNITRO.KN_set_con_eqbnds(kc, collect(Cint, ind_con_dyn .- 1), zeros(m₁))
+        KNITRO.KN_set_con_eqbnds(kc, m₁, collect(Cint, ind_con_dyn .- 1), zeros(m₁))
 
         if use_inv_dyn
             jac = problem.jacdata_inv_dyn.jac
@@ -100,7 +100,7 @@ function solve_with_knitro(problem::Problem, robot::Robot;
 
         @assert length(ind_con_ee_pos) == length(con_ee)
 
-        KNITRO.KN_set_con_eqbnds(kc, collect(Cint, ind_con_ee_pos .- 1), vec(con_ee))
+        KNITRO.KN_set_con_eqbnds(kc, length(con_ee), collect(Cint, ind_con_ee_pos .- 1), vec(con_ee))
 
         cb = KNITRO.KN_add_eval_callback(kc, false, collect(Cint, ind_con_ee_pos .- 1), cb_eval_fc_con_ee)
 
@@ -155,7 +155,7 @@ function solve_with_knitro(problem::Problem, robot::Robot;
     @assert length(initial_guess) == n
 
     # Set starting solution
-    KNITRO.KN_set_var_primal_init_values(kc, initial_guess)
+    KNITRO.KN_set_var_primal_init_values_all(kc, initial_guess)
 
     # # # # # # # # #
     # User Options  #