[WIP] Add orientation constraints to arbitrary links of mechanisms

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"

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()'
+```

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

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.

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)
 
@@ -112,7 +120,8 @@ def main():
             initial_guess=jl_initial_guess,
             user_options=ipopt_options,
             use_inv_dyn=True,
-            minimise_τ=False,
+            minimise_velocities=False,
+            minimise_torques=False,
         )
 
         # Unpack the solution `x` into joint positions, velocities, and torques

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")')

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,15 +238,15 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Julia 1.11.1",
+   "display_name": "Julia 1.11.3",
    "language": "julia",
    "name": "julia-1.11"
   },
   "language_info": {
    "file_extension": ".jl",
    "mimetype": "application/julia",
    "name": "julia",
-   "version": "1.11.1"
+   "version": "1.11.3"
   }
  },
  "nbformat": 4,

notebooks/Tutorial.ipynb

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

src/TORA.jl

@@ -10,6 +10,7 @@ using NPZ
 using Plots
 using Requires
 using RigidBodyDynamics
+using Rotations
 using SparseArrays
 using SparseDiffTools
 using StaticArrays
@@ -23,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.
@@ -33,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)

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

src/precompile.jl

@@ -1,13 +1,60 @@
-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))
+
+        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_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)
+
+        Plots.closeall()
+        MeshCat.close_server!(vis.core)
+
+    end
+
 end

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