Add instructions on how to use this Julia package from Python

Author: ferrolho

.gitignore

@@ -25,3 +25,6 @@ Manifest.toml
 
 # Jupyter
 .ipynb_checkpoints/
+
+# Conda
+.CondaPkg/

interoperability/README.md

@@ -0,0 +1,42 @@
+# Interoperability
+
+**Table of Contents**
+- [Interoperability](#interoperability)
+  - [Python and Julia in harmony](#python-and-julia-in-harmony)
+    - [Calling Julia code from Python code](#calling-julia-code-from-python-code)
+
+## Python and Julia in harmony
+
+Call Python code from Julia and Julia code from Python via a symmetric interface.
+All details can be found in https://github.com/JuliaPy/PythonCall.jl.
+
+### Calling Julia code from Python code
+
+First, install the `juliacall` Python module with:
+```bash
+pip install juliacall
+```
+
+Then, in your Python script, import the `juliacall` module with:
+```python
+from juliacall import Main as jl
+```
+
+Next, navigate into the `interoperability` folder:
+```bash
+cd /path/to/TORA.jl/interoperability
+```
+
+Run the setup script to install the example dependencies:
+```bash
+python python_setup.py
+```
+
+Finally, run the example script with:
+```bash
+python python_calls_julia.py
+```
+
+Use `CTRL + C` at any point to stop running the example code.
+
+The example script has comments explaining what is going on, and can be used as the basis for using the Julia framework from a Python environment for interfacing with a real robot (or with a simulator).

interoperability/python_calls_julia.py

@@ -0,0 +1,134 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import time
+
+from juliacall import Main as jl, convert as jlconvert
+
+
+def init():
+    """
+    Initialize the Julia environment, load the TORA.jl package, and create a robot model.
+    """
+    # Load package dependencies
+    jl.seval("using MeshCat")
+    jl.seval("using TORA")
+
+    # Call the greet() function from the TORA.jl package
+    jl.TORA.greet()
+
+    jl_vis = jl.MeshCat.Visualizer()
+    # jl.open(jl_vis)  # NOT WORKING
+
+    jl_robot = jl.TORA.create_robot_franka("panda_arm", jl_vis)
+
+    return jl_robot
+
+
+def prepare_initial_guess(jl_problem, jl_robot, py_q):
+    # Start by assuming zero positions, velocities, and torques for all the knots of the trajectory
+    py_initial_qs = np.zeros((jl_problem.num_knots, jl_robot.n_q))
+    py_initial_vs = np.zeros((jl_problem.num_knots, jl_robot.n_v))
+    py_initial_τs = np.zeros((jl_problem.num_knots, jl_robot.n_τ))
+
+    # To improve the initial guess for the robot joint positions, we can take
+    # the current robot configuration and simply repeat it for all the knots
+    py_initial_qs = np.tile(py_q, (jl_problem.num_knots, 1))
+
+    # Concatenate the initial guess
+    py_initial_guess = np.concatenate((py_initial_qs, py_initial_vs, py_initial_τs), axis=1)
+
+    # Flatten the numpy array
+    py_initial_guess = py_initial_guess.flatten()
+
+    # Truncate the torques at the last knot of the trajectory from the initial guess
+    py_initial_guess = py_initial_guess[: -jl_robot.n_τ]
+
+    # Convert the numpy array to a Julia array
+    jl_initial_guess = jlconvert(jl.Vector[jl.Float64], py_initial_guess)
+
+    return jl_initial_guess
+
+
+def main():
+    jl.println("Hello from Julia!")
+
+    jl_robot = init()
+
+    trajectory_discretisation = 100  # in Hertz
+    trajectory_duration = 2.0  # in seconds
+
+    trajectory_num_knots = int(trajectory_discretisation * trajectory_duration) + 1  # in units (number of knots)
+    trajectory_dt = 1 / trajectory_discretisation  # in seconds
+
+    # Options for the Ipopt numerical solver
+    ipopt_options = jl.Dict()
+    ipopt_options["hessian_approximation"] = "limited-memory"  # L-BFGS
+    ipopt_options["max_cpu_time"] = 2.0  # in seconds
+    ipopt_options["mu_strategy"] = "adaptive"  # monotone, adaptive, quality-function
+    # ipopt_options["acceptable_compl_inf_tol"] = 0.1
+    # ipopt_options["acceptable_constr_viol_tol"] = 0.1
+    # ipopt_options["acceptable_dual_inf_tol"] = 1.0
+    ipopt_options["acceptable_iter"] = 1
+    ipopt_options["acceptable_tol"] = 1.0
+    # ipopt_options["print_level"] = 3  # default: 5
+
+    # Starting configuration of the robot
+    py_q_start = [0, 0, 0, -np.pi / 2, 0, np.pi, 0]
+
+    done = False
+
+    while not done:
+        # Create a problem instance
+        jl_problem = jl.TORA.Problem(jl_robot, trajectory_num_knots, trajectory_dt)
+
+        # Get the current robot state
+        # (but for this example, let's use a nominal configuration and random velocities)
+        py_q = py_q_start
+        py_v = np.random.rand(jl_robot.n_v)
+
+        # Constrain the initial joint positions and velocities to the current state of the robot
+        jl.TORA.fix_joint_positions_b(jl_problem, jl_robot, 1, py_q)
+        jl.TORA.fix_joint_velocities_b(jl_problem, jl_robot, 1, py_v)
+
+        # Constrain the final joint velocities to zero
+        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]
+
+        # 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)
+
+        # Show a summary of the problem instance (this can be commented out)
+        jl.TORA.show_problem_info(jl_problem)
+
+        # Prepare the initial guess for the solver
+        jl_initial_guess = prepare_initial_guess(jl_problem, jl_robot, py_q)
+
+        jl_cpu_time, jl_x, jl_solver_log = jl.TORA.solve_with_ipopt(
+            jl_problem,
+            jl_robot,
+            initial_guess=jl_initial_guess,
+            user_options=ipopt_options,
+            use_inv_dyn=True,
+            minimise_τ=False,
+        )
+
+        # Unpack the solution `x` into joint positions, velocities, and torques
+        jl_qs, jl_vs, jl_τs = jl.TORA.unpack_x(jl_x, jl_robot, jl_problem)
+
+        # Convert the Julia arrays to numpy arrays
+        py_qs = np.array(jl_qs)
+        py_vs = np.array(jl_vs)
+        py_τs = np.array(jl_τs)
+
+        # Do something with the result (e.g., control the real robot)
+        # (...)
+
+        # Sleep for 1 second before the next iteration
+        time.sleep(1)
+
+
+if __name__ == "__main__":
+    main()

interoperability/python_setup.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+
+# Import the Main class from the juliacall module
+from juliacall import Main as jl
+
+# Import the Pkg module
+jl.seval("import Pkg")
+
+# Check the status of the installed packages
+jl.seval("Pkg.status()")
+
+# Add package dependencies
+jl.seval('Pkg.add("MeshCat")')
+
+# Develop the local package of TORA.jl
+jl.seval('Pkg.develop(path="../../TORA.jl")')
+
+# Update the package dependencies
+jl.seval("Pkg.update()")
+
+# Import the TORA.jl package
+jl.seval("using TORA")
+
+# Test the TORA.jl package by calling the greet() function
+jl.TORA.greet()

src/TORA.jl

@@ -44,7 +44,7 @@ function __init__()
     @require KNITRO = "67920dd8-b58e-52a8-8622-53c4cffbe346" include("./transcription/knitro.jl")
 end
 
-greet() = print("Hello World!")
+greet() = println("Hello World!")
 
 # Artifacts (robot meshes, URDF files, etc.)
 include("../dev/artifacts.jl")
@@ -60,6 +60,7 @@ include("./constraints/dynamics.jl")
 include("./constraints/end_effector.jl")
 include("./transcription/ipopt.jl")
 include("./plots.jl")
+include("./utils.jl")
 
 # Code to "exercise" the package - see https://julialang.github.io/PrecompileTools.jl/stable/
 include("./precompile.jl")

src/utils.jl

@@ -0,0 +1,14 @@
+function unpack_x(x::Vector, robot::Robot, problem::Problem)
+    nₓ = robot.n_q + robot.n_v + robot.n_τ  # dimension of each mesh point
+
+    # Helper ranges
+    ind_q = hcat([range(1 + (i * nₓ)                        , length=robot.n_q) for i = (1:problem.num_knots    ) .- 1]...)
+    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]...)
+
+    qs = x[ind_q]
+    vs = x[ind_v]
+    τs = x[ind_τ]
+
+    return qs, vs, τs
+end