diff --git a/.gitmodules b/.gitmodules index 06bbeb4..4a418c8 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "pybind"] path = pybind url = git@github.com:pybind/pybind11.git +[submodule "pid"] + path = pid + url = git@github.com:lbr-stack/pid.git diff --git a/CMakeLists.txt b/CMakeLists.txt index c599f50..87a6490 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/README.md b/README.md index 51e68f9..6fa9c7f 100644 --- a/README.md +++ b/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 + +

+ +

+ +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: diff --git a/doc/sync-vs-async.png b/doc/sync-vs-async.png new file mode 100644 index 0000000..1ee1341 Binary files /dev/null and b/doc/sync-vs-async.png differ diff --git a/examples/async_example.py b/examples/async_example.py new file mode 100644 index 0000000..3616ee6 --- /dev/null +++ b/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() diff --git a/pid b/pid new file mode 160000 index 0000000..ac2e11e --- /dev/null +++ b/pid @@ -0,0 +1 @@ +Subproject commit ac2e11e684de5fe54aa88e5a7cced81796b43dac diff --git a/pyFRI/src/async_client.cpp b/pyFRI/src/async_client.cpp new file mode 100644 index 0000000..7933479 --- /dev/null +++ b/pyFRI/src/async_client.cpp @@ -0,0 +1,234 @@ +// Standard library +#include +#include + +// KUKA FRI-Client-SDK_Cpp (using version hosted at: +// https://github.com/cmower/FRI-Client-SDK_Cpp) +#include "friClientApplication.h" +#include "friLBRClient.h" +#include "friUdpConnection.h" + +// PID implementation: https://github.com/cmower/pid +#include "pid.hpp" + +const unsigned int NCART = 6; // number of dimensions in cartesian vector +const unsigned int NDOF = + KUKA::FRI::LBRState::NUMBER_OF_JOINTS; // number of degrees of freedom + +class AsyncLBRClient : public KUKA::FRI::LBRClient { + +private: + bool _ready; + + KUKA::FRI::EClientCommandMode _ccmode; + + double _dt; + + double _set_position[NDOF]; + double _set_wrench[NCART]; + double _set_torque[NDOF]; + + double _pv_position[NDOF]; + double _pv_wrench[NCART]; + double _pv_torque[NDOF]; + + PIDControl::PID _pid_position[NDOF]; + PIDControl::PID _pid_wrench[NCART]; + PIDControl::PID _pid_torque[NDOF]; + + bool _is_position_pid_ready() { + for (auto &pid : _pid_position) { + if (!pid.is_ready()) + return false; + } + return true; + } + + bool _is_wrench_pid_ready() { + for (auto &pid : _pid_wrench) { + if (!pid.is_ready()) + return false; + } + return true; + } + + bool _is_torque_pid_ready() { + for (auto &pid : _pid_torque) { + if (!pid.is_ready()) + return false; + } + return true; + } + + void _command() { + + // Command position + robotCommand().setJointPosition(_pv_position); + + // Command wrench/torque + switch (_ccmode) { + + case KUKA::FRI::EClientCommandMode::WRENCH: { + robotCommand().setWrench(_pv_wrench); + break; + } + + case KUKA::FRI::EClientCommandMode::TORQUE: { + robotCommand().setTorque(_pv_torque); + break; + } + } + } + + void _update() { + + // When not ready simply return + if (!_ready) + return; + + // Update PID + for (unsigned int i = 0; i < NDOF; ++i) + _pv_position[i] = + _pid_position[i].next(_set_position[i], _pv_position[i], _dt); + + switch (_ccmode) { + + case KUKA::FRI::EClientCommandMode::WRENCH: { + for (unsigned int i = 0; i < NCART; ++i) + _pv_wrench[i] = _pid_wrench[i].next(_set_wrench[i], _pv_wrench[i], _dt); + break; + } + + case KUKA::FRI::EClientCommandMode::TORQUE: { + for (unsigned int i = 0; i < NDOF; ++i) + _pv_torque[i] = _pid_torque[i].next(_set_torque[i], _pv_torque[i], _dt); + break; + } + } + } + +public: + AsyncLBRClient() : _ready(false) { + + // Fill set/pv position and torque with zeros + for (unsigned i = 0; i < NDOF; ++i) { + _set_position[i] = 0.0; + _pv_position[i] = 0.0; + _set_torque[i] = 0.0; + _pv_torque[i] = 0.0; + } + + // Fill set/pv wrench with zeros + for (unsigned i = 0; i < NCART; ++i) { + _set_wrench[i] = 0.0; + _pv_wrench[i] = 0.0; + } + } + + ~AsyncLBRClient() {} + + void set_pid_position_gains(double Kp[NDOF], double Ki[NDOF], + double Kd[NDOF]) { + for (unsigned int i = 0; i < NDOF; ++i) + _pid_position[i].set_gains(Kp[i], Ki[i], Kd[i]); + } + + void set_pid_wrench_gains(double Kp[NCART], double Ki[NCART], + double Kd[NCART]) { + for (unsigned int i = 0; i < NCART; ++i) + _pid_wrench[i].set_gains(Kp[i], Ki[i], Kd[i]); + } + + void set_pid_torque_gains(double Kp[NDOF], double Ki[NDOF], double Kd[NDOF]) { + for (unsigned int i = 0; i < NDOF; ++i) + _pid_torque[i].set_gains(Kp[i], Ki[i], Kd[i]); + } + + void set_position(double position[NDOF]) { + for (unsigned int i = 0; i < NDOF; ++i) { + _set_position[i] = position[i]; + } + } + + void set_wrench(double wrench[NCART]) { + for (unsigned int i = 0; i < NCART; ++i) { + _set_wrench[i] = wrench[i]; + } + } + + void set_torque(double torque[NDOF]) { + for (unsigned int i = 0; i < NDOF; ++i) { + _set_torque[i] = torque[i]; + } + } + + void onStateChange(KUKA::FRI::ESessionState oldState, + KUKA::FRI::ESessionState newState) { + + // Report state change + KUKA::FRI::LBRClient::onStateChange(oldState, newState); + + // Set set/pv position + if (newState == KUKA::FRI::ESessionState::MONITORING_READY) { + + // Get sample time + _dt = robotState().getSampleTime(); + + // Get client command mode + _ccmode = robotState().getClientCommandMode(); + + // Retrieve current joint position + memcpy(_pv_position, robotState().getIpoJointPosition(), + NDOF * sizeof(double)); + + // Initialize set position + for (unsigned int i = 0; i < NDOF; ++i) { + _set_position[i] = _pv_position[i]; + + // Reset position/torque PID + _pid_position[i].reset(); + _pid_torque[i].reset(); + } + + // Reset wrench PID + for (unsigned int i = 0; i < NCART; ++i) + _pid_wrench[i].reset(); + + // Ensure gains are set for position PID controller + if (!_is_position_pid_ready()) { + std::cout << "Error: you must set gains for PID position controller.\n"; + return; + } + + // Ensure gains are set for wrench/torque PID controllers + switch (_ccmode) { + + case KUKA::FRI::EClientCommandMode::WRENCH: { + if (!_is_wrench_pid_ready()) { + std::cout << "Error: you must set gains for PID wrench controller.\n"; + return; + } + } + + case KUKA::FRI::EClientCommandMode::TORQUE: { + if (!_is_torque_pid_ready()) { + std::cout << "Error: you must set gains for PID torque controller.\n"; + return; + } + } + } + + // Above checks passed -> client is ready + _ready = true; + } + } + + void monitor() { KUKA::FRI::LBRClient::monitor(); } + + void waitForCommand() { _command(); } + + void command() { + _update(); + _command(); + } +}; diff --git a/pyFRI/src/async_client_application.cpp b/pyFRI/src/async_client_application.cpp new file mode 100644 index 0000000..b488f7e --- /dev/null +++ b/pyFRI/src/async_client_application.cpp @@ -0,0 +1,97 @@ +// Standard library +#include +#include + +// KUKA FRI-Client-SDK_Cpp (using version hosted at: +// https://github.com/cmower/FRI-Client-SDK_Cpp) +#include "friClientApplication.h" +#include "friLBRClient.h" +#include "friUdpConnection.h" + +// Local +#include "async_client.cpp" + +// Asynchronous client application implementation +class AsyncClientApplication { + +private: + bool _connected; + bool _fri_spinning; + + std::thread _fri_loop_thread; + + struct sigaction _sig_int_handler; + + KUKA::FRI::UdpConnection _connection; + AsyncLBRClient &_client; + KUKA::FRI::ClientApplication &_app; + + void _spin_fri() { + + while (_connected) { + + // Step the application + _connected = _app.step(); + + // Update _fri_spinning variable + if (_connected) + _fri_spinning = true; + else + _fri_spinning = false; + + // Get session state + KUKA::FRI::ESessionState state = _client.robotState().getSessionState(); + + // Check session state + if (state == KUKA::FRI::ESessionState::IDLE) { + _connected = false; + _fri_spinning = false; + } + } + } + +public: + AsyncClientApplication() + : _client(*new AsyncLBRClient), + _app(*new KUKA::FRI::ClientApplication(_connection, _client)), + _connected(false), _fri_spinning(false) { + _sig_int_handler.sa_handler = + &AsyncClientApplication::wait_exception_handler; + sigemptyset(&_sig_int_handler.sa_mask); + _sig_int_handler.sa_flags = 0; + sigaction(SIGINT, &_sig_int_handler, NULL); + } + + static void wait_exception_handler(int s) { + throw std::runtime_error("quitting async client application"); + } + + bool connect(const int port, char *const remoteHost = NULL) { + + // Connect to controller + _connected = _app.connect(port, remoteHost); + + // When successfull, start the fri loop + if (_connected) + _fri_loop_thread = std::thread(&AsyncClientApplication::_spin_fri, this); + + return _connected; + } + + bool is_ok() { return _connected && _fri_spinning; } + + void disconnect() { + _app.disconnect(); + _connected = false; + _fri_spinning = false; + } + + AsyncLBRClient client() { return _client; } + + void wait() { + while (!_fri_spinning) { + std::this_thread::sleep_for( + std::chrono::milliseconds(10)); // Sleep for 10 milliseconds + } + } +}; diff --git a/pyFRI/src/wrapper.cpp b/pyFRI/src/wrapper.cpp index becb98c..db1e1e9 100644 --- a/pyFRI/src/wrapper.cpp +++ b/pyFRI/src/wrapper.cpp @@ -20,6 +20,9 @@ #include "friUdpConnection.h" #include "fri_config.h" +// Local +#include "async_client_application.cpp" + // Function for returning the current time long long getCurrentTimeInNanoseconds() { using namespace std::chrono; @@ -34,6 +37,43 @@ long long getCurrentTimeInNanoseconds() { return duration.count(); } +// +// Rate class +// +// Based on rospy.Rate implementation: +// https://github.com/ros/ros_comm/blob/noetic-devel/clients/rospy/src/rospy/timer.py +// +class Rate { + +public: + Rate(float hz) { + float sleep_dur = 1000000000.0 / hz; + _sleep_dur = static_cast(sleep_dur); + _last_time = getCurrentTimeInNanoseconds(); + } + + void sleep() { + long long curr_time = getCurrentTimeInNanoseconds(); + std::this_thread::sleep_for( + std::chrono::nanoseconds(_remaining(curr_time))); + _last_time += _sleep_dur; + if (curr_time - _last_time > _sleep_dur * 2) + _last_time = curr_time; + } + +private: + long long _last_time; + long long _sleep_dur; + + long long _remaining(long long curr_time) { + + if (_last_time > curr_time) + _last_time = curr_time; + + return _sleep_dur - (curr_time - _last_time); + } +}; + // Make LBRClient a Python abstract class class PyLBRClient : public KUKA::FRI::LBRClient { @@ -199,9 +239,30 @@ class PyClientApplication { } }; -// Python bindings +// Define py namespace namespace py = pybind11; +// Helper methods +void check_py_array(py::array_t arr, int size) { + if (arr.ndim() != 1 || PyArray_DIMS(arr.ptr())[0] != size) { + std::string errmsg = "Array must have shape ("; + errmsg += std::to_string(size); + errmsg += ",)."; + throw std::runtime_error(errmsg); + } +} + +double *cvt_py_array(py::array_t arr, int size) { + check_py_array(arr, size); + return static_cast(arr.request().ptr); +} + +void not_exposed(std::string name) { + throw std::runtime_error(name + " is not yet exposed."); +} + +// Python bindings + PYBIND11_MODULE(_pyFRI, m) { m.doc() = "Python bindings for the KUKA FRI Client SDK. THIS IS NOT A KUKA " "PRODUCT."; @@ -413,52 +474,32 @@ PYBIND11_MODULE(_pyFRI, m) { #elif FRI_VERSION_MAJOR == 2 .def("getMeasuredCartesianPose", [](const KUKA::FRI::LBRState &self) { - - // Declare variables - double data[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; - float dataf[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; - - // Retrieve state - memcpy(data, self.getMeasuredCartesianPose(), - KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); - - // Parse: double -> float - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) - dataf[i] = (float)data[i]; - - return py::array_t({KUKA::FRI::LBRState::NUMBER_OF_JOINTS}, - dataf); + not_exposed("getMeasuredCartesianPose"); }) .def("getMeasuredCartesianPoseAsMatrix", [](const KUKA::FRI::LBRState &self) { - // TODO - throw std::runtime_error("getMeasuredCartesianPoseAsMatrix is not yet exposed (use .getMeasuredCartesianPose instead)."); + not_exposed("getMeasuredCartesianPoseAsMatrix"); }) .def("getIpoCartesianPose", [](const KUKA::FRI::LBRState &self) { - // TODO - // Currently, FRI Cartesian Overlay is not supported by FRI-Client-SDK_Python. - // IPO Cartesian Pose not available when FRI Cartesian Overlay is not active. - throw std::runtime_error("getIpoCartesianPose is not yet exposed."); + not_exposed("getIpoCartesianPose"); }) .def("getIpoCartesianPoseAsMatrix", [](const KUKA::FRI::LBRState &self) { - // TODO - // Currently, FRI Cartesian Overlay is not supported by FRI-Client-SDK_Python. - // IPO Cartesian Pose not available when FRI Cartesian Overlay is not active. - throw std::runtime_error("getIpoCartesianPoseAsMatrix is not yet exposed."); + not_exposed("getIpoCartesianPoseAsMatrix"); }) .def("getMeasuredRedundancyValue", - &KUKA::FRI::LBRState::getMeasuredRedundancyValue) + [](KUKA::FRI::LBRState &self) { + not_exposed("getMeasuredRedundancyValue"); + }) .def("getIpoRedundancyValue", [](KUKA::FRI::LBRState &self) { - // TODO - // Currently, FRI Cartesian Overlay is not supported by FRI-Client-SDK_Python. - // IPO redundancy value not available when FRI Cartesian Overlay is not active. - throw std::runtime_error("getIpoRedundancyValue is not yet exposed."); + not_exposed("getIpoRedundancyValue"); }) .def("getRedundancyStrategy", - &KUKA::FRI::LBRState::getRedundancyStrategy) + [](KUKA::FRI::LBRState &self) { + not_exposed("getRedundancyStrategy"); + }) #endif ; // NOTE: this completes LBRState @@ -466,61 +507,23 @@ PYBIND11_MODULE(_pyFRI, m) { .def(py::init<>()) .def("setJointPosition", [](KUKA::FRI::LBRCommand &self, py::array_t values) { - if (values.ndim() != 1 || - PyArray_DIMS(values.ptr())[0] != - KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - throw std::runtime_error( - "Input array must have shape (" + - std::to_string(KUKA::FRI::LBRState::NUMBER_OF_JOINTS) + - ",)!"); - } - auto buf = values.request(); - const double *data = static_cast(buf.ptr); - self.setJointPosition(data); + self.setJointPosition(cvt_py_array(values, NDOF)); }) .def("setWrench", [](KUKA::FRI::LBRCommand &self, py::array_t values) { - if (values.ndim() != 1 || - PyArray_DIMS(values.ptr())[0] != - 6 // [F_x, F_y, F_z, tau_A, tau_B, tau_C] - ) { - throw std::runtime_error( - "Input array must have shape (" + - std::to_string(KUKA::FRI::LBRState::NUMBER_OF_JOINTS) + - ",)!"); - } - auto buf = values.request(); - const double *data = static_cast(buf.ptr); - self.setWrench(data); + self.setWrench(cvt_py_array(values, NCART)); }) .def("setTorque", [](KUKA::FRI::LBRCommand &self, py::array_t values) { - if (values.ndim() != 1 || - PyArray_DIMS(values.ptr())[0] != - KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - throw std::runtime_error( - "Input array must have shape (" + - std::to_string(KUKA::FRI::LBRState::NUMBER_OF_JOINTS) + - ",)!"); - } - auto buf = values.request(); - const double *data = static_cast(buf.ptr); - self.setTorque(data); + self.setTorque(cvt_py_array(values, NDOF)); }) .def("setCartesianPose", [](KUKA::FRI::LBRCommand &self, py::array_t values) { - // TODO - // Currently, FRI Cartesian Overlay is not supported by - // FRI-Client-SDK_Python. - throw std::runtime_error("setCartesianPose is not yet exposed."); + not_exposed("setCartesianPose"); }) .def("setCartesianPoseAsMatrix", [](KUKA::FRI::LBRCommand &self, py::array_t values) { - // TODO - // Currently, FRI Cartesian Overlay is not supported by - // FRI-Client-SDK_Python. - throw std::runtime_error( - "setCartesianPoseAsMatrix is not yet exposed."); + not_exposed("setCartesianPoseAsMatrix"); }) .def("setBooleanIOValue", &KUKA::FRI::LBRCommand::setBooleanIOValue) .def("setDigitalIOValue", &KUKA::FRI::LBRCommand::setDigitalIOValue) @@ -541,4 +544,56 @@ PYBIND11_MODULE(_pyFRI, m) { .def("collect_data", &PyClientApplication::collect_data) .def("disconnect", &PyClientApplication::disconnect) .def("step", &PyClientApplication::step); + + py::class_(m, "Rate").def(py::init()).def("sleep", &Rate::sleep); + + py::class_(m, "AsyncClient") + .def(py::init<>()) + .def("set_pid_position_gains", + [](AsyncLBRClient &self, py::array_t Kp, + py::array_t Ki, py::array_t Kd) { + self.set_pid_position_gains(cvt_py_array(Kp, NDOF), + cvt_py_array(Ki, NDOF), + cvt_py_array(Kd, NDOF)); + }) + .def("set_pid_wrench_gains", + [](AsyncLBRClient &self, py::array_t Kp, + py::array_t Ki, py::array_t Kd) { + self.set_pid_wrench_gains(cvt_py_array(Kp, NCART), + cvt_py_array(Ki, NCART), + cvt_py_array(Kd, NCART)); + }) + .def("set_pid_torque_gains", + [](AsyncLBRClient &self, py::array_t Kp, + py::array_t Ki, py::array_t Kd) { + self.set_pid_torque_gains(cvt_py_array(Kp, NDOF), + cvt_py_array(Ki, NDOF), + cvt_py_array(Kd, NDOF)); + }) + .def("robotState", &AsyncLBRClient::robotState) + .def("set_position", + [](AsyncLBRClient &self, py::array_t position) { + self.set_position(cvt_py_array(position, NDOF)); + }) + .def("set_wrench", + [](AsyncLBRClient &self, py::array_t wrench) { + self.set_wrench(cvt_py_array(wrench, NCART)); + }) + .def("set_torque", [](AsyncLBRClient &self, py::array_t torque) { + self.set_torque(cvt_py_array(torque, NDOF)); + }); + + py::class_(m, "AsyncClientApplication") + .def(py::init([]() { + auto app = + new AsyncClientApplication(); // Create a new instance using new + std::unique_ptr ptr( + app); // Wrap it in a unique_ptr + return ptr; // Return the unique_ptr + })) + .def("connect", &AsyncClientApplication::connect) + .def("wait", &AsyncClientApplication::wait) + .def("is_ok", &AsyncClientApplication::is_ok) + .def("client", &AsyncClientApplication::client) + .def("disconnect", &AsyncClientApplication::disconnect); }