Add async support

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "pybind"]
 	path = pybind
 	url = [email protected]:pybind/pybind11.git
+[submodule "pid"]
+	path = pid
+	url = [email protected]:lbr-stack/pid.git

CMakeLists.txt

@@ -9,10 +9,12 @@ project(_pyFRI)
 
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 set(FRI_BUILD_EXAMPLES OFF)
+set(PID_BUILD_EXAMPLES OFF)
 
 add_subdirectory(pybind)
 add_subdirectory(FRI-Client-SDK_Cpp)
+add_subdirectory(pid)
 
 pybind11_add_module(_pyFRI ${CMAKE_CURRENT_SOURCE_DIR}/pyFRI/src/wrapper.cpp)
 
-target_link_libraries(_pyFRI PRIVATE FRIClient)
+target_link_libraries(_pyFRI PRIVATE FRIClient pid pthread)

README.md

@@ -30,6 +30,8 @@ app = fri.ClientApplication(client)
 
 Since UDP is the only supported connection type and the connection object is not actually required by the user after declaring the variable, the `UdpConnection` object is created internally to the `fri.ClientApplication` class object.
 
+The `pyFRI` library also supports asynchronous execution, see *Execution types* section below.
+
 See the [examples](examples/).
 
 # Important notice
@@ -38,6 +40,49 @@ See the [examples](examples/).
 
 [@cmower](https://github.com/cmower) is not affiliated with KUKA.
 
+# Execution types
+
+<p align="center">
+  <img src="doc/sync-vs-async.png" width="70%" align="center">
+</p>
+
+Two execution types are supported: (i) synchronous, and (ii) asynchronous.
+These are both shown in the figure above.
+
+## Synchronous
+
+This constitutes the operational approach embraced by FRI.
+Conceptually, you can envision this approach as executing the subsequent actions:
+
+1. The KUKA controller sends a message to the LBR client over a UDP connection.
+2. A response is computed (using some method defined in the client application).
+3. The commanded response is encoded and sent back to the controller.
+4. The physical robot moves according the command and controller type selected in the Java application.
+
+These steps are repeated at a sampling rate defined in the Java application, e.g. 200Hz.
+
+The pyFRI library abstracts the functionalities of the `ClientApplication` and `LBRClient` classes, enabling users to craft application scripts using classes/functions that mirror the examples provided in the FRI C++ documentation.
+An added benefit is the availability of KUKA's FRI documentation for C++, which can serve as a guide for pyFRI users.
+
+The drawback for this approach is the execution loop in the Python application must fit within the sampling frequency set by the Java application.
+As such, higher sampling frequencies (i.e. 500-1000Hz) can be difficult to achieve using pyFRI.
+
+The majority of the examples use the synchronous execution style.
+
+## Asynchronous
+
+The pyFRI library incorporates an asynchronous execution approach, allowing users to execute FRI communication at various permissible sampling frequencies (i.e., 100-1000Hz), along with a distinct sampling frequency for the loop on the Python application's end.
+The FRI communication on the C++ side is executed on another thread and shared memory between the C++ client and Python application is used to define the target joint states.
+
+In order to ensure smooth robot motion, a PID controller is implemented where the user specifies the set target.
+The process variable is executed on the robot using an open-loop PID controller.
+
+The advantage of employing this execution approach lies in the flexibility to configure the controller to operate at the user's preferred frequency, while the Python loop can operate at a lower frequency.
+This proves particularly useful during when implementing Model Predictive Control.
+However, a downside of this method is the necessity for precise tuning of the PID controller.
+
+See the [examples/async_example.py](examples/async_example.py) example script.
+
 # Support
 
 The following versions of FRI are currently supported:

examples/async_example.py

@@ -0,0 +1,87 @@
+import time
+import math
+import argparse
+import numpy as np
+import pyFRI as fri
+
+np.set_printoptions(precision=3, suppress=True, linewidth=1000)
+
+
+def get_arguments():
+    def cvt_joint_mask(value):
+        int_value = int(value)
+        if 0 <= int_value < 7:
+            return int_value
+        else:
+            raise argparse.ArgumentTypeError(f"{value} is not in the range [0, 7).")
+
+    parser = argparse.ArgumentParser(description="LRBJointSineOverlay example.")
+    parser.add_argument(
+        "--hostname",
+        dest="hostname",
+        default=None,
+        help="The hostname used to communicate with the KUKA Sunrise Controller.",
+    )
+    parser.add_argument(
+        "--port",
+        dest="port",
+        type=int,
+        default=30200,
+        help="The port number used to communicate with the KUKA Sunrise Controller.",
+    )
+    parser.add_argument(
+        "--joint-mask",
+        dest="joint_mask",
+        type=cvt_joint_mask,
+        default=3,
+        help="The joint to move.",
+    )
+
+    return parser.parse_args()
+
+
+def main():
+    print("Running FRI Version:", fri.FRI_VERSION)
+
+    # Get arguments and initialize client application
+    args = get_arguments()
+    app = fri.AsyncClientApplication()
+
+    # Set PID position gains
+    pos_Kp = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
+    pos_Ki = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+    pos_Kd = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+    app.client().set_pid_position_gains(pos_Kp, pos_Ki, pos_Kd)
+
+    # Connect to controller
+    if app.connect(args.port, args.hostname):
+        print("Connected to KUKA Sunrise controller.")
+    else:
+        print("Connection to KUKA Sunrise controller failed.")
+        return
+
+    # Wait for FRI loop to start
+    app.wait()
+    print("FRI Loop started")
+
+    # Setup for Python loop
+    hz = 10
+    dt = 1.0 / float(hz)
+    rate = fri.Rate(hz)
+    q = app.client().robotState().getIpoJointPosition()
+
+    try:
+        t = 0.0
+        while app.is_ok():
+            q[args.joint_mask] += math.radians(20) * math.sin(t * 0.01)
+            app.client().set_position(q.astype(np.float32))
+            rate.sleep()
+            t += time_step
+    except KeyboardInterrupt:
+        pass
+    finally:
+        app.disconnect()
+
+
+if __name__ == "__main__":
+    main()