From 181d407110462938496e8a8f9c127e7fd0978936 Mon Sep 17 00:00:00 2001
From: Henrique Ferrolho <henrique.ferrolho@gmail.com>
Date: Sun, 9 Feb 2025 15:30:28 +0800
Subject: [PATCH 1/6] Update precompile.jl to decreate TTFX

---
 src/precompile.jl | 67 ++++++++++++++++++++++++++++++++++++++---------
 1 file changed, 55 insertions(+), 12 deletions(-)

diff --git a/src/precompile.jl b/src/precompile.jl
index 94509e9..1ced205 100644
--- a/src/precompile.jl
+++ b/src/precompile.jl
@@ -1,13 +1,56 @@
-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)
+using PrecompileTools: @setup_workload, @compile_workload
+
+# PrecompileTools allows you to reduce the latency of the first execution of Julia code.
+# It is applicable for package developers and for "ordinary users" in their personal workflows.
+# See the GitHub repository and docs for more details: https://github.com/JuliaLang/PrecompileTools.jl.
+
+@setup_workload begin
+
+    # Putting some things in `@setup_workload` instead of `@compile_workload` can
+    # reduce the size of the precompile file and potentially make loading faster.
+
+    # CAREFUL! Objects defined here will persist in every session including this package.
+
+    @compile_workload begin
+
+        # All calls in this block will be precompiled, regardless of whether they belong to this package or not.
+
+        # Options for the Ipopt numerical solver
+        ipopt_options = Dict(
+            # "acceptable_compl_inf_tol" => 0.1,
+            # "acceptable_constr_viol_tol" => 0.1,
+            # "acceptable_dual_inf_tol" => 1.0,
+            "acceptable_iter" => 1,
+            "acceptable_tol" => 1.0,
+            "hessian_approximation" => "limited-memory",
+            # "max_cpu_time" => 15.0,
+            "max_iter" => 1,
+            "mu_strategy" => "monotone",
+            "print_level" => 0,  # default: 5
+        )
+
+        vis = Visualizer()
+        robot = create_robot_franka("panda_arm", vis)
+        problem = Problem(robot, 2, 1 / 100)
+
+        initial_q = Float64[0, 0, 0, -π/2, 0, π, 0]
+
+        fix_joint_positions!(problem, robot, 1, initial_q)
+        fix_joint_velocities!(problem, robot, 1, zeros(robot.n_v))
+        constrain_ee_position!(problem, 1, zeros(3))
+
+        initial_guess = zeros(problem.num_knots * (robot.n_q + robot.n_v + robot.n_τ) - robot.n_τ)
+
+        cpu_time, x, solver_log = solve_with_ipopt(problem, robot, initial_guess=initial_guess, user_options=ipopt_options, use_inv_dyn=false, minimise_τ=false)
+        cpu_time, x, solver_log = solve_with_ipopt(problem, robot, initial_guess=initial_guess, user_options=ipopt_options, use_inv_dyn=false, minimise_τ=true)
+        cpu_time, x, solver_log = solve_with_ipopt(problem, robot, initial_guess=initial_guess, user_options=ipopt_options, use_inv_dyn=true, minimise_τ=false)
+
+        play_trajectory(vis, problem, robot, x)
+        plot_results(problem, robot, x)
+
+        Plots.closeall()
+        MeshCat.close_server!(vis.core)
+
+    end
+
 end

From 5660076e1090a0d10168aa13b6a59f68d8afc228 Mon Sep 17 00:00:00 2001
From: Henrique Ferrolho <henrique.ferrolho@gmail.com>
Date: Sun, 9 Feb 2025 17:17:18 +0800
Subject: [PATCH 2/6] [WIP] Add ability to constrain the orientation of
 mechanism links

---
 Project.toml                           |  2 +
 interoperability/python_calls_julia.py | 12 +++-
 interoperability/python_setup.py       |  1 +
 src/TORA.jl                            |  1 +
 src/constraints/end_effector.jl        | 14 +++++
 src/precompile.jl                      |  4 ++
 src/problem.jl                         | 58 +++++++++++++++++-
 src/transcription/ipopt.jl             | 85 +++++++++++++++++++++++---
 8 files changed, 164 insertions(+), 13 deletions(-)

diff --git a/Project.toml b/Project.toml
index 6d9b077..932de84 100644
--- a/Project.toml
+++ b/Project.toml
@@ -18,6 +18,7 @@ PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 Requires = "ae029012-a4dd-5104-9daa-d747884805df"
 RigidBodyDynamics = "366cf18f-59d5-5db9-a4de-86a9f6786172"
+Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
 SparseDiffTools = "47a9eef4-7e08-11e9-0b38-333d64bd3804"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
@@ -34,6 +35,7 @@ Plots = "1"
 PrecompileTools = "1"
 Requires = "1"
 RigidBodyDynamics = "2"
+Rotations = "1"
 SparseDiffTools = "2"
 StaticArrays = "1"
 julia = "1.10"
diff --git a/interoperability/python_calls_julia.py b/interoperability/python_calls_julia.py
index 57d1cf7..08034cf 100644
--- a/interoperability/python_calls_julia.py
+++ b/interoperability/python_calls_julia.py
@@ -12,6 +12,7 @@ def init():
     """
     # Load package dependencies
     jl.seval("using MeshCat")
+    jl.seval("using Rotations")
     jl.seval("using TORA")
 
     # Call the greet() function from the TORA.jl package
@@ -55,7 +56,7 @@ def main():
 
     jl_robot = init()
 
-    trajectory_discretisation = 100  # in Hertz
+    trajectory_discretisation = 10  # in Hertz
     trajectory_duration = 2.0  # in seconds
 
     trajectory_num_knots = int(trajectory_discretisation * trajectory_duration) + 1  # in units (number of knots)
@@ -95,11 +96,18 @@ def main():
         jl.TORA.fix_joint_velocities_b(jl_problem, jl_robot, jl_problem.num_knots, np.zeros(jl_robot.n_v))
 
         # Define the end-effector position that we want to achieve
-        py_eff_pos = [0.5, 0.2, 0.1]
+        py_eff_pos = [0.5, 0.2, 0.8]
 
         # Constrain the end-effector position at the last knot of the trajectory
         jl.TORA.constrain_ee_position_b(jl_problem, jl_problem.num_knots, py_eff_pos)
 
+        # Define the end-effector orientation that we want to achieve
+        body_name = "panda_link7"  # this is the fixed parent link of "panda_hand_tcp"
+        jl_quaternion = jl.QuatRotation(0.27, 0.65, 0.27, 0.65)  # Quaternion order is (w, x, y, z)
+
+        # Constrain the end-effector orientation at the last knot of the trajectory
+        jl.TORA.add_constraint_body_orientation_b(jl_problem, jl_robot, body_name, jl_problem.num_knots, jl_quaternion)
+
         # Show a summary of the problem instance (this can be commented out)
         jl.TORA.show_problem_info(jl_problem)
 
diff --git a/interoperability/python_setup.py b/interoperability/python_setup.py
index bbaa37f..1626786 100644
--- a/interoperability/python_setup.py
+++ b/interoperability/python_setup.py
@@ -11,6 +11,7 @@
 
 # Add package dependencies
 jl.seval('Pkg.add("MeshCat")')
+jl.seval('Pkg.add("Rotations")')
 
 # Develop the local package of TORA.jl
 jl.seval('Pkg.develop(path="../../TORA.jl")')
diff --git a/src/TORA.jl b/src/TORA.jl
index 0d1b0d4..fbca4e9 100644
--- a/src/TORA.jl
+++ b/src/TORA.jl
@@ -10,6 +10,7 @@ using NPZ
 using Plots
 using Requires
 using RigidBodyDynamics
+using Rotations
 using SparseArrays
 using SparseDiffTools
 using StaticArrays
diff --git a/src/constraints/end_effector.jl b/src/constraints/end_effector.jl
index 123b1cd..03699c6 100644
--- a/src/constraints/end_effector.jl
+++ b/src/constraints/end_effector.jl
@@ -64,3 +64,17 @@ function show_ee_path(vis::Visualizer, ee_positions::Matrix{Float64})
     setobject!(vis["ee path"], Object(PointCloud(points), material, "Line"))
     nothing
 end
+
+function body_orientation!(dx, robot, x::AbstractVector{T}) where {T}
+    state = robot.statecache[T]
+    set_configuration!(state, x)
+
+    body_tf = transform_to_root(state, robot.frame_ee)
+    mrp = MRP(rotation(body_tf))
+
+    dx[1] = mrp.x
+    dx[2] = mrp.y
+    dx[3] = mrp.z
+
+    dx
+end
diff --git a/src/precompile.jl b/src/precompile.jl
index 1ced205..37d709b 100644
--- a/src/precompile.jl
+++ b/src/precompile.jl
@@ -39,6 +39,10 @@ using PrecompileTools: @setup_workload, @compile_workload
         fix_joint_velocities!(problem, robot, 1, zeros(robot.n_v))
         constrain_ee_position!(problem, 1, zeros(3))
 
+        body_name = "panda_link7"  # this is the fixed parent link of "panda_hand_tcp"
+        quaternion = QuatRotation(0.27, 0.65, 0.27, 0.65)
+        add_constraint_body_orientation!(problem, robot, body_name, 1, quaternion)
+
         initial_guess = zeros(problem.num_knots * (robot.n_q + robot.n_v + robot.n_τ) - robot.n_τ)
 
         cpu_time, x, solver_log = solve_with_ipopt(problem, robot, initial_guess=initial_guess, user_options=ipopt_options, use_inv_dyn=false, minimise_τ=false)
diff --git a/src/problem.jl b/src/problem.jl
index 8e32671..6017553 100644
--- a/src/problem.jl
+++ b/src/problem.jl
@@ -22,6 +22,9 @@ mutable struct Problem
 
     ee_pos::Dict{Int64,Point{3,Float64}}  # End-effector target positions
 
+    # Dictionaries indexing: knot → body_name → value
+    constraints_body_orientation::Dict{Int64,Dict{String,Rotation}}
+
     # Jacobian data (e.g., for sparse Jacobian of dynamics with AD)
     const jacdata_fwd_dyn::JacobianData
     const jacdata_inv_dyn::JacobianData
@@ -58,6 +61,7 @@ mutable struct Problem
             Dict{Int64,Vector{Float64}}(),
             Dict{Int64,Vector{Float64}}(),
             Dict{Int64,Point{3,Float64}}(),
+            Dict{Int64,Dict{String,Rotation}}(),
             jacdata_fwd_dyn,
             jacdata_inv_dyn,
             jacdata_ee_position
@@ -121,6 +125,55 @@ function constrain_ee_position!(problem::Problem, knot, position)
     return problem
 end
 
+"""
+    add_constraint_body_orientation!(problem, robot, body_name, knot, rotation; force=false)
+
+Constrain the orientation of a body (kinematic link) of the robot.
+
+See also: [`add_constraint_body_position!`](@ref)
+"""
+function add_constraint_body_orientation!(problem::Problem, robot::Robot, body_name::String, knot::Integer, rotation::Rotation; force=false)
+    @assert body_name ∈ map(x -> x.name, bodies(robot.mechanism))
+    @assert 1 <= knot <= problem.num_knots
+
+    d = problem.constraints_body_orientation
+
+    if !haskey(d, knot)
+        d[knot] = Dict{String,Rotation}()
+    end
+
+    dₖ = d[knot]
+
+    if !haskey(dₖ, body_name) || force
+        dₖ[body_name] = rotation
+    elseif haskey(dₖ, body_name)
+        @warn """
+        You tried to add a constraint for `$(body_name)` at knot=$(knot),
+        but you have already defined a constraint for that knot before.
+        If you wish to overwrite previous constraints, use `force=true`.
+        """
+    end
+
+    return problem
+end
+
+"""
+    body_orientation_constraints_per_knot(problem)
+
+Count number of transcription constraints needed to enforce the
+orientation constraints of the high-level problem definition.
+"""
+function body_orientation_constraints_per_knot(problem::Problem)
+    map(1:problem.num_knots) do knot
+        counter = 0
+        dₖ = get(problem.constraints_body_orientation, knot, Dict())
+        for (body_name, rotation) ∈ dₖ
+            counter += 3  # MRP values
+        end
+        counter
+    end
+end
+
 """
     show_problem_info(problem)
 
@@ -129,12 +182,15 @@ Output a summary of the problem, including the number of knots with constraints.
 function show_problem_info(problem::Problem)
     t = (problem.num_knots - 1) * problem.dt
 
+    println("Discretization ....................................... $(problem.num_knots) knots")
     println("Motion duration ...................................... $(t) seconds")
-    println("Number of knots ...................................... $(problem.num_knots)")
+    println("Time step length ..................................... $(problem.dt) seconds")
+    println()
     println("Number of knots with constrained joint positions ..... $(length(problem.fixed_q))")
     println("Number of knots with constrained joint velocities .... $(length(problem.fixed_v))")
     println("Number of knots with constrained joint torques ....... $(length(problem.fixed_τ))")
     println("Number of knots with constrained ee position ......... $(length(problem.ee_pos))")
+    println("Number of knots with body orientation constraints .... $(length(problem.constraints_body_orientation))")
 end
 
 """
diff --git a/src/transcription/ipopt.jl b/src/transcription/ipopt.jl
index a1e10ce..588f101 100644
--- a/src/transcription/ipopt.jl
+++ b/src/transcription/ipopt.jl
@@ -73,11 +73,13 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
 
     use_m₁ = true  # nonlinear equality: dynamics
     use_m₂ = true  # nonlinear equality: ee xyz-position
+    use_m₃ = true  # nonlinear equality: ee rotation
 
     m₁ = !use_m₁ ? 0 : (robot.n_q + robot.n_v) * (problem.num_knots - 1)  # equations of motion
     m₂ = !use_m₂ ? 0 : 3 * length(problem.ee_pos)  # xyz-position (3 NL equality)
+    m₃ = !use_m₃ ? 0 : body_orientation_constraints_per_knot(problem) |> sum
 
-    m = m₁ + m₂  # total number of constraints
+    m = m₁ + m₂ + m₃  # total number of constraints
 
     g_L, g_U = Float64[], Float64[]
 
@@ -93,7 +95,28 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
         append!(g_U, vec(con_ee))
     end
 
-    @assert length(g_L) == length(g_U)
+    body_name = "panda_link7"  # TODO ... hard-coded for now.
+    # We want to iterate only through the constrained knots for this robot body/link.
+    knots = filter(1:problem.num_knots) do knotᵢ
+        knotᵢ ∈ keys(problem.constraints_body_orientation) &&
+        body_name ∈ keys(problem.constraints_body_orientation[knotᵢ])
+    end
+
+    jacdata = JacobianData((out, x) -> body_orientation!(out, robot, x), rand(3), rand(robot.n_q))
+
+    if use_m₃
+        # These are the target values for the equalities of Knitro.
+        con_vals = mapreduce(hcat, knots; init=Matrix{Float64}(undef, 3, 0)) do k
+            dₖ = problem.constraints_body_orientation[k]
+            mrp = MRP(dₖ[body_name])
+            [mrp.x, mrp.y, mrp.z]
+        end
+
+        append!(g_L, con_vals)
+        append!(g_U, con_vals)
+    end
+
+    @assert length(g_L) == length(g_U) == m
     @assert eltype(g_L) == eltype(g_U) == Float64
 
     # dimension of each mesh point
@@ -122,7 +145,7 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
         end
 
         if use_m₂
-            length_c = 3
+            length_c = size(problem.jacdata_ee_position.jac, 1)  # length of constraint (per knot)
             for (i, k) = enumerate(sort(collect(keys(problem.ee_pos))))
                 ind_vars = range(1 + (k - 1) * nₓ, length=robot.n_q)  # indices of the decision variables
                 offset_con = (i - 1) * length_c + (m₁)
@@ -130,6 +153,17 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
                 @views ee_position!(g[ind_cons], robot, x[ind_vars])  # constraint evaluation at that point
             end
         end
+
+        if use_m₃
+            length_c = size(jacdata.jac, 1)  # length of constraint (per knot)
+            offset_con = m₁ + m₂
+            for i = knots .- 1
+                ind_cons = (1:length_c) .+ offset_con                      # Indices of the respective constraints
+                ind_vars = range(1 + i * nₓ, length=robot.n_q)             # Indices of the decision variables
+                @views body_orientation!(g[ind_cons], robot, x[ind_vars])  # Constraint evaluation at that point
+                offset_con += length_c
+            end
+        end
     end
 
     jacIndexConsCB = Cint[]
@@ -167,6 +201,22 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
         @assert length(jacIndexConsCB) == length(jacIndexVarsCB)
     end
 
+    if use_m₃
+        ind_offset = m₁ + m₂
+        for k = knots
+            inds = rowvals(jacdata.jac) .+ ind_offset
+            ind_offset += size(jacdata.jac, 1)
+            append!(jacIndexConsCB, inds)
+        end
+
+        for k = knots
+            vals = vcat([fill(j + (k - 1) * nₓ, length(nzrange(jacdata.jac, j)))
+                         for j = 1:size(jacdata.jac, 2)]...)
+            append!(jacIndexVarsCB, vals)
+        end
+
+        @assert length(jacIndexConsCB) == length(jacIndexVarsCB)
+    end
 
     function eval_jac_g(x, rows, cols, values::Union{Nothing,Vector{Float64}})
         if isnothing(values)
@@ -186,12 +236,11 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
                     # Evaluate the Jacobian at that point
                     jacdata_dyn(x[ind_vars])
 
-                    # Pass the Jacobian to Knitro
-                    offset_jac = i * jacdata_dyn.jac_length
-                    ind_jac = (1:jacdata_dyn.jac_length) .+ offset_jac
+                    # Pass the Jacobian to Ipopt
+                    ind_jac = (1:jacdata_dyn.jac_length) .+ offset_prev
                     values[ind_jac] = nonzeros(jacdata_dyn.jac)
+                    offset_prev += jacdata_dyn.jac_length
                 end
-                offset_prev += (problem.num_knots - 1) * jacdata_dyn.jac_length
             end
 
             if use_m₂
@@ -199,9 +248,24 @@ 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.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
+
+                    # Pass the Jacobian to Ipopt
+                    ind_jac = (1:problem.jacdata_ee_position.jac_length) .+ offset_prev
+                    values[ind_jac] = nonzeros(problem.jacdata_ee_position.jac)
+                    offset_prev += problem.jacdata_ee_position.jac_length
+                end
+            end
+
+            if use_m₃
+                # End-effector constraints (rotation)
+                for k = knots
+                    ind_qᵢ = range(1 + (k - 1) * nₓ, length=robot.n_q)  # Indices of the decision variables
+                    jacdata(x[ind_qᵢ])  # Evaluate the Jacobian at that point
+
+                    # Pass the Jacobian to Ipopt
+                    ind_jac = (1:jacdata.jac_length) .+ offset_prev
+                    values[ind_jac] = nonzeros(jacdata.jac)
+                    offset_prev += jacdata.jac_length
                 end
             end
         end
@@ -244,6 +308,7 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
     nele_jac = 0
     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)
+    nele_jac += !use_m₃ ? 0 : jacdata.jac_length * length(knots)
 
     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)

From d7deba9dc203f8ac23be895f18c11758af3b68ff Mon Sep 17 00:00:00 2001
From: Henrique Ferrolho <henrique.ferrolho@gmail.com>
Date: Wed, 12 Feb 2025 13:48:12 +0000
Subject: [PATCH 3/6] =?UTF-8?q?Rename=20minimise=5F=CF=84=20to=20minimise?=
 =?UTF-8?q?=5Ftorques?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 interoperability/python_calls_julia.py | 2 +-
 notebooks/Example.ipynb                | 8 ++++----
 notebooks/Tutorial.ipynb               | 4 ++--
 src/TORA.jl                            | 4 ++--
 src/precompile.jl                      | 6 +++---
 src/transcription/ipopt.jl             | 8 ++++----
 src/transcription/knitro.jl            | 4 ++--
 7 files changed, 18 insertions(+), 18 deletions(-)

diff --git a/interoperability/python_calls_julia.py b/interoperability/python_calls_julia.py
index 08034cf..6fbf124 100644
--- a/interoperability/python_calls_julia.py
+++ b/interoperability/python_calls_julia.py
@@ -120,7 +120,7 @@ def main():
             initial_guess=jl_initial_guess,
             user_options=ipopt_options,
             use_inv_dyn=True,
-            minimise_τ=False,
+            minimise_torques=False,
         )
 
         # Unpack the solution `x` into joint positions, velocities, and torques
diff --git a/notebooks/Example.ipynb b/notebooks/Example.ipynb
index 020abd0..76ad235 100644
--- a/notebooks/Example.ipynb
+++ b/notebooks/Example.ipynb
@@ -153,7 +153,7 @@
    "outputs": [],
    "source": [
     "use_inv_dyn = true\n",
-    "minimise_τ = false\n",
+    "minimise_torques = false\n",
     "\n",
     "user_options = Dict(\n",
     "    # === Termination === #  https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Termination\n",
@@ -202,7 +202,7 @@
     "cpu_time, x, solver_log = solve(problem, robot,\n",
     "                                initial_guess=initial_guess,\n",
     "                                use_inv_dyn=use_inv_dyn,\n",
-    "                                minimise_τ=minimise_τ,\n",
+    "                                minimise_torques=minimise_torques,\n",
     "                                user_options=user_options)"
    ]
   },
@@ -238,7 +238,7 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Julia 1.11.1",
+   "display_name": "Julia 1.11.3",
    "language": "julia",
    "name": "julia-1.11"
   },
@@ -246,7 +246,7 @@
    "file_extension": ".jl",
    "mimetype": "application/julia",
    "name": "julia",
-   "version": "1.11.1"
+   "version": "1.11.3"
   }
  },
  "nbformat": 4,
diff --git a/notebooks/Tutorial.ipynb b/notebooks/Tutorial.ipynb
index 6abdeac..67e2207 100644
--- a/notebooks/Tutorial.ipynb
+++ b/notebooks/Tutorial.ipynb
@@ -128,7 +128,7 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Julia 1.11.1",
+   "display_name": "Julia 1.11.3",
    "language": "julia",
    "name": "julia-1.11"
   },
@@ -136,7 +136,7 @@
    "file_extension": ".jl",
    "mimetype": "application/julia",
    "name": "julia",
-   "version": "1.11.1"
+   "version": "1.11.3"
   }
  },
  "nbformat": 4,
diff --git a/src/TORA.jl b/src/TORA.jl
index fbca4e9..e6c890c 100644
--- a/src/TORA.jl
+++ b/src/TORA.jl
@@ -24,7 +24,7 @@ import Base: length
     solve_with_knitro(problem, robot;
                       initial_guess=Float64[],
                       use_inv_dyn=false,
-                      minimise_τ=false,
+                      minimise_torques=false,
                       user_options=Dict())
 
 Solve the nonlinear optimization problem with Knitro.
@@ -34,7 +34,7 @@ Further options can be set using the keyword arguments. See [Solver Interfaces](
 # Keyword arguments
 - `initial_guess::Vector{Float64}=Float64[]`: the starting point for the solver.
 - `use_inv_dyn::Bool=false`: if true, enables the use of inverse dynamics instead of forward dynamics.
-- `minimise_τ::Bool=false`: if true, activates a cost function to minimize the joint torques.
+- `minimise_torques::Bool=false`: if true, activates a cost function to minimize the joint torques.
 - `user_options::Dict=Dict()`: the user options for Knitro.
 
 See also: [`solve_with_ipopt`](@ref)
diff --git a/src/precompile.jl b/src/precompile.jl
index 37d709b..c55cb5b 100644
--- a/src/precompile.jl
+++ b/src/precompile.jl
@@ -45,9 +45,9 @@ using PrecompileTools: @setup_workload, @compile_workload
 
         initial_guess = zeros(problem.num_knots * (robot.n_q + robot.n_v + robot.n_τ) - robot.n_τ)
 
-        cpu_time, x, solver_log = solve_with_ipopt(problem, robot, initial_guess=initial_guess, user_options=ipopt_options, use_inv_dyn=false, minimise_τ=false)
-        cpu_time, x, solver_log = solve_with_ipopt(problem, robot, initial_guess=initial_guess, user_options=ipopt_options, use_inv_dyn=false, minimise_τ=true)
-        cpu_time, x, solver_log = solve_with_ipopt(problem, robot, initial_guess=initial_guess, user_options=ipopt_options, use_inv_dyn=true, minimise_τ=false)
+        cpu_time, x, solver_log = solve_with_ipopt(problem, robot, initial_guess=initial_guess, user_options=ipopt_options, use_inv_dyn=false, minimise_torques=false)
+        cpu_time, x, solver_log = solve_with_ipopt(problem, robot, initial_guess=initial_guess, user_options=ipopt_options, use_inv_dyn=false, minimise_torques=true)
+        cpu_time, x, solver_log = solve_with_ipopt(problem, robot, initial_guess=initial_guess, user_options=ipopt_options, use_inv_dyn=true, minimise_torques=false)
 
         play_trajectory(vis, problem, robot, x)
         plot_results(problem, robot, x)
diff --git a/src/transcription/ipopt.jl b/src/transcription/ipopt.jl
index 588f101..daeba3c 100644
--- a/src/transcription/ipopt.jl
+++ b/src/transcription/ipopt.jl
@@ -6,7 +6,7 @@ addOption(prob, k, v::Float64) = Ipopt.AddIpoptNumOption(prob, k, v)
     solve_with_ipopt(problem, robot;
                      initial_guess=Float64[],
                      use_inv_dyn=false,
-                     minimise_τ=false,
+                     minimise_torques=false,
                      user_options=Dict())
 
 Solve the nonlinear optimization problem with Ipopt.
@@ -16,7 +16,7 @@ Further options can be set using the keyword arguments. See [Solver Interfaces](
 # Keyword arguments
 - `initial_guess::Vector{Float64}=Float64[]`: the starting point for the solver.
 - `use_inv_dyn::Bool=false`: if true, enables the use of inverse dynamics instead of forward dynamics.
-- `minimise_τ::Bool=false`: if true, activates a cost function to minimize the joint torques.
+- `minimise_torques::Bool=false`: if true, activates a cost function to minimize the joint torques.
 - `user_options::Dict=Dict()`: the user options for Ipopt.
 
 See also: [`solve_with_knitro`](@ref)
@@ -24,7 +24,7 @@ See also: [`solve_with_knitro`](@ref)
 function solve_with_ipopt(problem::Problem, robot::Robot;
                           initial_guess::Vector{Float64}=Float64[],
                           use_inv_dyn::Bool=false,
-                          minimise_τ::Bool=false,
+                          minimise_torques::Bool=false,
                           user_options::Dict=Dict("hessian_approximation" => "limited-memory"))
     # # # # # # # # # # # # # # # #
     # Variables and their bounds  #
@@ -283,7 +283,7 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
         grad_f[:] = zeros(n)
     end
 
-    if minimise_τ
+    if minimise_torques
         ind_τ = hcat([range(1 + (i * nₓ) + robot.n_q + robot.n_v, length=robot.n_τ)
                       for i = (1:problem.num_knots - 1) .- 1]...)
         indexVars, coefs = vec(ind_τ), fill(1 / (problem.num_knots - 1), n3)
diff --git a/src/transcription/knitro.jl b/src/transcription/knitro.jl
index de6d7ad..c23c56a 100644
--- a/src/transcription/knitro.jl
+++ b/src/transcription/knitro.jl
@@ -3,7 +3,7 @@ using .KNITRO
 function solve_with_knitro(problem::Problem, robot::Robot;
                            initial_guess::Vector{Float64}=Float64[],
                            use_inv_dyn::Bool=false,
-                           minimise_τ::Bool=false,
+                           minimise_torques::Bool=false,
                            user_options::Dict=Dict())
     lm = KNITRO.LMcontext()  # Instantiate license manager
     kc = KNITRO.KN_new_lm(lm)  # Create a new Knitro instance
@@ -136,7 +136,7 @@ function solve_with_knitro(problem::Problem, robot::Robot;
     # Objective #
     # # # # # # #
 
-    if minimise_τ
+    if minimise_torques
         # Minimize τ, i.e., the necessary joint torques.
         indexVars, coefs = Cint.(vec(ind_τ) .- 1), fill(1 / (problem.num_knots - 1), n3)
         KNITRO.KN_add_obj_quadratic_struct(kc, indexVars, indexVars, coefs)

From 99ff12b0d34af494126be28e0279b6495ce7088a Mon Sep 17 00:00:00 2001
From: Henrique Ferrolho <henrique.ferrolho@gmail.com>
Date: Wed, 12 Feb 2025 13:49:28 +0000
Subject: [PATCH 4/6] Add option to minimise joint velocities

---
 interoperability/python_calls_julia.py |  1 +
 src/transcription/ipopt.jl             | 40 ++++++++++++++++++--------
 2 files changed, 29 insertions(+), 12 deletions(-)

diff --git a/interoperability/python_calls_julia.py b/interoperability/python_calls_julia.py
index 6fbf124..5827df5 100644
--- a/interoperability/python_calls_julia.py
+++ b/interoperability/python_calls_julia.py
@@ -120,6 +120,7 @@ def main():
             initial_guess=jl_initial_guess,
             user_options=ipopt_options,
             use_inv_dyn=True,
+            minimise_velocities=False,
             minimise_torques=False,
         )
 
diff --git a/src/transcription/ipopt.jl b/src/transcription/ipopt.jl
index daeba3c..bb48c83 100644
--- a/src/transcription/ipopt.jl
+++ b/src/transcription/ipopt.jl
@@ -6,6 +6,7 @@ addOption(prob, k, v::Float64) = Ipopt.AddIpoptNumOption(prob, k, v)
     solve_with_ipopt(problem, robot;
                      initial_guess=Float64[],
                      use_inv_dyn=false,
+                     minimise_velocities=false,
                      minimise_torques=false,
                      user_options=Dict())
 
@@ -16,6 +17,7 @@ Further options can be set using the keyword arguments. See [Solver Interfaces](
 # Keyword arguments
 - `initial_guess::Vector{Float64}=Float64[]`: the starting point for the solver.
 - `use_inv_dyn::Bool=false`: if true, enables the use of inverse dynamics instead of forward dynamics.
+- `minimise_velocities::Bool=false`: if true, activates a cost function to minimize the joint velocities.
 - `minimise_torques::Bool=false`: if true, activates a cost function to minimize the joint torques.
 - `user_options::Dict=Dict()`: the user options for Ipopt.
 
@@ -24,6 +26,7 @@ See also: [`solve_with_knitro`](@ref)
 function solve_with_ipopt(problem::Problem, robot::Robot;
                           initial_guess::Vector{Float64}=Float64[],
                           use_inv_dyn::Bool=false,
+                          minimise_velocities::Bool=false,
                           minimise_torques::Bool=false,
                           user_options::Dict=Dict("hessian_approximation" => "limited-memory"))
     # # # # # # # # # # # # # # # #
@@ -275,28 +278,41 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
     # Objective #
     # # # # # # #
 
-    function eval_f(x)
-        return 0
-    end
+    eval_f(x) = 0  # Callback function for evaluating objective function
+    eval_grad_f(x, grad_f) = grad_f[:] = zeros(n)  # Callback function for evaluating gradient of objective function
+
+    if minimise_velocities
+        offset_velocities = robot.n_q + 1  # indice of the first velocity value within a knot point
+        indexVars = vcat([range((i - 1) * nₓ + offset_velocities, length=robot.n_v)
+                          for i = 1:problem.num_knots]...)
+        coefs = fill(1 / problem.num_knots, robot.n_v * problem.num_knots)
+        @assert length(indexVars) == length(coefs)
+
+        eval_f = function (x)
+            return sum(coefs .* x[indexVars] .* x[indexVars])
+        end
 
-    function eval_grad_f(x, grad_f)
-        grad_f[:] = zeros(n)
+        eval_grad_f = function (x, grad_f)
+            grad_f[:] = zeros(n)
+            @. grad_f[indexVars] = coefs * 2 * x[indexVars]
+        end
     end
 
     if minimise_torques
-        ind_τ = hcat([range(1 + (i * nₓ) + robot.n_q + robot.n_v, length=robot.n_τ)
-                      for i = (1:problem.num_knots - 1) .- 1]...)
-        indexVars, coefs = vec(ind_τ), fill(1 / (problem.num_knots - 1), n3)
+        offset_torques = robot.n_q + robot.n_v + 1  # indice of the first torque value within a knot point
+        num_knots_with_torque = problem.num_knots - 1
+        indexVars = vcat([range((i - 1) * nₓ + offset_torques, length=robot.n_τ)
+                          for i = 1:num_knots_with_torque]...)
+        coefs = fill(1 / num_knots_with_torque, robot.n_τ * num_knots_with_torque)
         @assert length(indexVars) == length(coefs)
 
-        eval_f = function(x)
-            # Minimize τ, i.e., the necessary joint torques.
+        eval_f = function (x)
             return sum(coefs .* x[indexVars] .* x[indexVars])
         end
 
-        eval_grad_f = function(x, grad_f)
+        eval_grad_f = function (x, grad_f)
             grad_f[:] = zeros(n)
-            grad_f[indexVars] .= coefs .* 2x[indexVars]
+            @. grad_f[indexVars] = coefs * 2 * x[indexVars]
         end
     end
 

From 948374daf1aaf6121549252dd50b952dcd8250c5 Mon Sep 17 00:00:00 2001
From: Henrique Ferrolho <henrique.ferrolho@gmail.com>
Date: Wed, 12 Feb 2025 13:49:38 +0000
Subject: [PATCH 5/6] Update docs

---
 docs/README.md                | 19 +++++++++++++++++++
 docs/make.jl                  |  1 +
 docs/src/solver_interfaces.md |  4 ++--
 3 files changed, 22 insertions(+), 2 deletions(-)
 create mode 100644 docs/README.md

diff --git a/docs/README.md b/docs/README.md
new file mode 100644
index 0000000..2554c22
--- /dev/null
+++ b/docs/README.md
@@ -0,0 +1,19 @@
+Navigate to the root of the project with
+```bash
+cd path/to/TORA.jl
+```
+
+To build the docs locally run
+```bash
+julia --color=yes --project=docs docs/make.jl
+```
+
+To serve the docs locally run
+```bash
+julia -e 'using LiveServer; serve(dir="docs/build")'
+```
+
+To serve the docs and update them in real time use
+```bash
+julia --project=docs -ie 'using TORA, LiveServer; servedocs()'
+```
diff --git a/docs/make.jl b/docs/make.jl
index c406919..82f1cb8 100644
--- a/docs/make.jl
+++ b/docs/make.jl
@@ -7,6 +7,7 @@ makedocs(
     format = Documenter.HTML(),
     warnonly = Documenter.except(),
     sitename = "TORA.jl",
+    # repo = Remotes.GitHub("JuliaRobotics", "TORA.jl"),
     authors = "Henrique Ferrolho",
     pages = [
         "Home" => "index.md",
diff --git a/docs/src/solver_interfaces.md b/docs/src/solver_interfaces.md
index 2188bdf..7993b22 100644
--- a/docs/src/solver_interfaces.md
+++ b/docs/src/solver_interfaces.md
@@ -35,13 +35,13 @@ The *default value* for `use_inv_dyn` is `false`. I.e., the default behaviour is
 
 ### Energy Minimization
 
-`minimise_τ=false`
+`minimise_torques=false`
 
 By default, problems formulated with TORA.jl are *feasibility problems*.
 In other words, the goal for the solver is to find a set of values that satisfies all the constraints of a [`Problem`](@ref).
 However, it is possible to define a cost function to be minimized. (Or a value function to be maximized.)
 
-The *default value* for `minimise_τ` is `false`. I.e., the default behaviour is to solve the feasibility problem, without optimizing any objective function. Alternatively, if `minimise_τ` is set to `true`, TORA.jl will minimize the joint torques required by the trajectory being computed.
+The *default value* for `minimise_torques` is `false`. I.e., the default behaviour is to solve the feasibility problem, without optimizing any objective function. Alternatively, if `minimise_torques` is set to `true`, TORA.jl will minimize the joint torques required by the trajectory being computed.
 
 !!! warning
     Optimizing an objective function is very costly, and may take a significant amount of iterations depending on the [Initial Guess](@ref) provided to the solver.

From 98e2be650f5994f07921a257f851fd95f2a1600d Mon Sep 17 00:00:00 2001
From: Henrique Ferrolho <henrique.ferrolho@gmail.com>
Date: Wed, 12 Feb 2025 13:50:06 +0000
Subject: [PATCH 6/6] Temporarily disable hard-coded body_name for orientation
 constraints in ipopt.jl

---
 src/transcription/ipopt.jl | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/transcription/ipopt.jl b/src/transcription/ipopt.jl
index bb48c83..1f27363 100644
--- a/src/transcription/ipopt.jl
+++ b/src/transcription/ipopt.jl
@@ -98,11 +98,12 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
         append!(g_U, vec(con_ee))
     end
 
-    body_name = "panda_link7"  # TODO ... hard-coded for now.
     # We want to iterate only through the constrained knots for this robot body/link.
     knots = filter(1:problem.num_knots) do knotᵢ
-        knotᵢ ∈ keys(problem.constraints_body_orientation) &&
-        body_name ∈ keys(problem.constraints_body_orientation[knotᵢ])
+        # body_name = "panda_link7"  # TODO ... hard-coded for now.
+        # knotᵢ ∈ keys(problem.constraints_body_orientation) &&
+        # body_name ∈ keys(problem.constraints_body_orientation[knotᵢ])
+        knotᵢ ∈ keys(problem.constraints_body_orientation)
     end
 
     jacdata = JacobianData((out, x) -> body_orientation!(out, robot, x), rand(3), rand(robot.n_q))
@@ -111,7 +112,8 @@ function solve_with_ipopt(problem::Problem, robot::Robot;
         # These are the target values for the equalities of Knitro.
         con_vals = mapreduce(hcat, knots; init=Matrix{Float64}(undef, 3, 0)) do k
             dₖ = problem.constraints_body_orientation[k]
-            mrp = MRP(dₖ[body_name])
+            # mrp = MRP(dₖ[body_name])
+            mrp = MRP(dₖ |> values |> only)
             [mrp.x, mrp.y, mrp.z]
         end