diff --git a/CMakeLists.txt b/CMakeLists.txt index 87ff0fa..a547341 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,8 +20,11 @@ endif() set(EXTRA_LIBS -pthread libunitree_legged_sdk.a) set(CMAKE_CXX_FLAGS "-O3 -fPIC") +set(CMAKE_C_FLAGS "-O3 -fPIC") set(CMAKE_CXX_STANDARD 14) +add_compile_options(-D BOOST_BIND_GLOBAL_PLACEHOLDERS) + find_package(catkin QUIET) if(${catkin_FOUND}) catkin_package( @@ -34,18 +37,23 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) # one pc one process add_executable(example_position example/example_position.cpp) +target_link_libraries(example_position PRIVATE "-no-pie") target_link_libraries(example_position ${EXTRA_LIBS}) add_executable(example_velocity example/example_velocity.cpp) +target_link_libraries(example_velocity PRIVATE "-no-pie") target_link_libraries(example_velocity ${EXTRA_LIBS}) add_executable(example_torque example/example_torque.cpp) +target_link_libraries(example_torque PRIVATE "-no-pie") target_link_libraries(example_torque ${EXTRA_LIBS}) add_executable(example_walk example/example_walk.cpp) +target_link_libraries(example_walk PRIVATE "-no-pie") target_link_libraries(example_walk ${EXTRA_LIBS}) add_executable(example_joystick example/example_joystick.cpp) +target_link_libraries(example_joystick PRIVATE "-no-pie") target_link_libraries(example_joystick ${EXTRA_LIBS}) diff --git a/lib/python/amd64/robot_interface.cpython-38-x86_64-linux-gnu.so b/lib/python/amd64/robot_interface.cpython-38-x86_64-linux-gnu.so index 9fa6b64..22660a0 100755 Binary files a/lib/python/amd64/robot_interface.cpython-38-x86_64-linux-gnu.so and b/lib/python/amd64/robot_interface.cpython-38-x86_64-linux-gnu.so differ diff --git a/python_wrapper/CMakeLists.txt b/python_wrapper/CMakeLists.txt index f1686d0..94a6ea5 100644 --- a/python_wrapper/CMakeLists.txt +++ b/python_wrapper/CMakeLists.txt @@ -10,7 +10,10 @@ if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "aarch64.*") set(ARCH arm64) endif() +add_compile_options(-D BOOST_BIND_GLOBAL_PLACEHOLDERS) + include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../include) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/third-party/pybind11/include) link_directories(${CMAKE_CURRENT_SOURCE_DIR}/../lib/cpp/${ARCH}) file(GLOB_RECURSE sources "cpp/*.cpp") diff --git a/python_wrapper/python_interface.cpp b/python_wrapper/python_interface.cpp index 510bd1b..a926f1e 100644 --- a/python_wrapper/python_interface.cpp +++ b/python_wrapper/python_interface.cpp @@ -68,7 +68,25 @@ PYBIND11_MODULE(robot_interface, m) { py::class_(m, "BmsCmd") .def(py::init<>()) .def_readwrite("off", &BmsCmd::off) - .def_readwrite("reserve", &BmsCmd::reserve); + .def_readwrite("reserve", &BmsCmd::reserve) + .def(py::pickle( + [](const BmsCmd &p) + { + // __getstate__ - Return a tuple that fully encodes the state of the object + return py::make_tuple(p.off, + p.reserve); + }, + [](py::tuple t) + { + // __setstate__ - Create a new C++ instance and assign state + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + BmsCmd p; + p.off = (t[0].cast()); + p.reserve = (t[1].cast>()); + return p; + } + )); py::class_(m, "BmsState") .def(py::init<>()) @@ -80,7 +98,39 @@ PYBIND11_MODULE(robot_interface, m) { .def_readwrite("cycle", &BmsState::cycle) .def_readwrite("BQ_NTC", &BmsState::BQ_NTC) .def_readwrite("MCU_NTC", &BmsState::MCU_NTC) - .def_readwrite("cell_vol", &BmsState::cell_vol); + .def_readwrite("cell_vol", &BmsState::cell_vol) + .def(py::pickle( + [](const BmsState &p) + { + // __getstate__ - Return a tuple that fully encodes the state of the object + return py::make_tuple(p.version_h, + p.version_l, + p.bms_status, + p.SOC, + p.current, + p.cycle, + p.BQ_NTC, + p.MCU_NTC, + p.cell_vol); + }, + [](py::tuple t) + { + // __setstate__ - Create a new C++ instance and assign state + if (t.size() != 9) + throw std::runtime_error("Invalid state!"); + BmsState p; + p.version_h = (t[0].cast()); + p.version_l = (t[1].cast()); + p.bms_status = (t[2].cast()); + p.SOC = (t[3].cast()); + p.current = (t[4].cast()); + p.cycle = (t[5].cast()); + p.BQ_NTC = (t[6].cast>()); + p.MCU_NTC = (t[7].cast>()); + p.cell_vol = (t[8].cast>()); + return p; + } + )); py::class_(m, "Cartesian") .def(py::init<>()) @@ -94,7 +144,31 @@ PYBIND11_MODULE(robot_interface, m) { .def_readwrite("gyroscope", &IMU::gyroscope) .def_readwrite("accelerometer", &IMU::accelerometer) .def_readwrite("rpy", &IMU::rpy) - .def_readwrite("temperature", &IMU::temperature); + .def_readwrite("temperature", &IMU::temperature) + .def(py::pickle( + [](const IMU &p) + { + // __getstate__ - Return a tuple that fully encodes the state of the object + return py::make_tuple(p.quaternion, + p.gyroscope, + p.accelerometer, + p.rpy, + p.temperature); + }, + [](py::tuple t) + { + // __setstate__ - Create a new C++ instance and assign state + if (t.size() != 5) + throw std::runtime_error("Invalid state!"); + IMU p; + p.quaternion = (t[0].cast>()); + p.gyroscope = (t[1].cast>()); + p.accelerometer = (t[2].cast>()); + p.rpy = (t[3].cast>()); + p.temperature = (t[4].cast()); + return p; + } + )); py::class_(m, "LED") .def(py::init<>()) @@ -113,7 +187,39 @@ PYBIND11_MODULE(robot_interface, m) { .def_readwrite("dq_raw", &MotorState::dq_raw) .def_readwrite("ddq_raw", &MotorState::ddq_raw) .def_readwrite("temperature", &MotorState::temperature) - .def_readwrite("reserve", &MotorState::reserve); + .def_readwrite("reserve", &MotorState::reserve) + .def(py::pickle( + [](const MotorState &p) { + // __getstate__ - Return a tuple that fully encodes the state of the object + return py::make_tuple(p.mode, + p.q, + p.dq, + p.ddq, + p.tauEst, + p.q_raw, + p.dq_raw, + p.ddq_raw, + p.temperature, + p.reserve); + }, + [](py::tuple t) { + // __setstate__ - Create a new C++ instance and assign state + if (t.size() != 10) + throw std::runtime_error("Invalid state!"); + MotorState p; + p.mode = (t[0].cast()); + p.q = (t[1].cast()); + p.dq = (t[2].cast()); + p.ddq = (t[3].cast()); + p.tauEst = (t[4].cast()); + p.q_raw = (t[5].cast()); + p.dq_raw = (t[6].cast()); + p.ddq_raw = (t[7].cast()); + p.temperature = (t[8].cast()); + p.reserve = (t[9].cast>()); + return p; + } + )); py::class_(m, "MotorCmd") .def(py::init<>()) @@ -123,7 +229,35 @@ PYBIND11_MODULE(robot_interface, m) { .def_readwrite("tau", &MotorCmd::tau) .def_readwrite("Kp", &MotorCmd::Kp) .def_readwrite("Kd", &MotorCmd::Kd) - .def_readwrite("reserve", &MotorCmd::reserve); + .def_readwrite("reserve", &MotorCmd::reserve) + .def(py::pickle( + [](const MotorCmd &p) + { + // __getstate__ - Return a tuple that fully encodes the state of the object + return py::make_tuple(p.mode, + p.q, + p.dq, + p.tau, + p.Kp, + p.Kd, + p.reserve); + }, + [](py::tuple t) + { + // __setstate__ - Create a new C++ instance and assign state + if (t.size() != 7) + throw std::runtime_error("Invalid state!"); + MotorCmd p; + p.mode = (t[0].cast()); + p.q = (t[1].cast()); + p.dq = (t[2].cast()); + p.tau = (t[3].cast()); + p.Kp = (t[4].cast()); + p.Kd = (t[5].cast()); + p.reserve = (t[6].cast>()); + return p; + } + )); py::class_(m, "LowState") .def(py::init<>()) @@ -141,7 +275,51 @@ PYBIND11_MODULE(robot_interface, m) { .def_readwrite("tick", &LowState::tick) .def_readwrite("wirelessRemote", &LowState::wirelessRemote) .def_readwrite("reserve", &LowState::reserve) - .def_readwrite("crc", &LowState::crc); + .def_readwrite("crc", &LowState::crc) + .def(py::pickle( + [](const LowState &p) + { + // __getstate__ - Return a tuple that fully encodes the state of the object + return py::make_tuple(p.head, + p.levelFlag, + p.frameReserve, + p.SN, + p.version, + p.bandWidth, + p.imu, + p.motorState, + p.bms, + p.footForce, + p.footForceEst, + p.tick, + p.wirelessRemote, + p.reserve, + p.crc); + }, + [](py::tuple t) + { + // __setstate__ - Create a new C++ instance and assign state + if (t.size() != 15) + throw std::runtime_error("Invalid state!"); + LowState p; + p.head = (t[0].cast>()); + p.levelFlag = (t[1].cast()); + p.frameReserve = (t[2].cast()); + p.SN = (t[3].cast>()); + p.version = (t[4].cast>()); + p.bandWidth = (t[5].cast()); + p.imu = (t[6].cast()); + p.motorState = (t[7].cast>()); + p.bms = (t[8].cast()); + p.footForce = (t[9].cast>()); + p.footForceEst = (t[10].cast>()); + p.tick = (t[11].cast()); + p.wirelessRemote = (t[12].cast>()); + p.reserve = (t[13].cast()); + p.crc = (t[14].cast()); + return p; + } + )); py::class_(m, "LowCmd") .def(py::init<>()) @@ -155,7 +333,43 @@ PYBIND11_MODULE(robot_interface, m) { .def_readwrite("bms", &LowCmd::bms) .def_readwrite("wirelessRemote", &LowCmd::wirelessRemote) .def_readwrite("reserve", &LowCmd::reserve) - .def_readwrite("crc", &LowCmd::crc); + .def_readwrite("crc", &LowCmd::crc) + .def(py::pickle( + [](const LowCmd &p) + { + // __getstate__ - Return a tuple that fully encodes the state of the object + return py::make_tuple(p.head, + p.levelFlag, + p.frameReserve, + p.SN, + p.version, + p.bandWidth, + p.motorCmd, + p.bms, + p.wirelessRemote, + p.reserve, + p.crc); + }, + [](py::tuple t) + { + // __setstate__ - Create a new C++ instance and assign state + if (t.size() != 11) + throw std::runtime_error("Invalid state!"); + LowCmd p; + p.head = (t[0].cast>()); + p.levelFlag = (t[1].cast()); + p.frameReserve = (t[2].cast()); + p.SN = (t[3].cast>()); + p.version = (t[4].cast>()); + p.bandWidth = (t[5].cast()); + p.motorCmd = (t[6].cast>()); + p.bms = (t[7].cast()); + p.wirelessRemote = (t[8].cast>()); + p.reserve = (t[9].cast()); + p.crc = (t[10].cast()); + return p; + } + )); py::class_(m, "HighState") .def(py::init<>())